Importing the code for relative ITS-TPC alignment (Mikolaj)
authorcvetan <cvetan@f7af4fe6-9843-0410-8265-dc069ae4e863>
Fri, 13 Jun 2008 21:09:43 +0000 (21:09 +0000)
committercvetan <cvetan@f7af4fe6-9843-0410-8265-dc069ae4e863>
Fri, 13 Jun 2008 21:09:43 +0000 (21:09 +0000)
STEER/AliRelAlignerKalman.cxx [new file with mode: 0644]
STEER/AliRelAlignerKalman.h [new file with mode: 0644]

diff --git a/STEER/AliRelAlignerKalman.cxx b/STEER/AliRelAlignerKalman.cxx
new file mode 100644 (file)
index 0000000..0aaa872
--- /dev/null
@@ -0,0 +1,1237 @@
+/**************************************************************************
+ * Copyright(c) 1998-1999, ALICE Experiment at CERN, All rights reserved. *
+ *                                                                        *
+ * Author: The ALICE Off-line Project.                                    *
+ * Contributors are mentioned in the code where appropriate.              *
+ *                                                                        *
+ * Permission to use, copy, modify and distribute this software and its   *
+ * documentation strictly for non-commercial purposes is hereby granted   *
+ * without fee, provided that the above copyright notice appears in all   *
+ * copies and that both the copyright notice and this permission notice   *
+ * appear in the supporting documentation. The authors make no claims     *
+ * about the suitability of this software for any purpose. It is          *
+ * provided "as is" without express or implied warranty.                  *
+ **************************************************************************/
+
+///////////////////////////////////////////////////////////////////////////////
+//
+//    Kalman filter based aligner:
+//    Aligns two tracking volumes (by default ITS and TPC)
+//    Determines the transformation of the second volume (TPC) wrt the first (ITS)
+//    additionally calculates some callibration parameters for TPC
+//    Fit parameters are:
+//    - 3 shifts, x,y,z
+//    - 3 Cardan angles, psi, theta, phi (see definition in alignment docs),
+//    - TPC drift velocity correction (vel_true = vel*(1+correction)),
+//    - TPC offset correction.
+//
+//    Basic usage:
+//    When aligning two volumes at any given time a single instance of
+//    the class should be active. The fit of the parameters is updated
+//    by adding new data using one of the Add.... methods.
+//
+//    User methods:
+//    In collision events add an ESD track to update the fit,
+//    track will be automatically split in ITS and TPC tracklets
+//    and the fit of the alignment constants will be updated:
+//    
+//        Bool_t AddESDTrack( AliESDtrack* pTrack );
+//    
+//    You may also give it the AliTrackPointArray,
+//    the fit will be updated if there are enough ITS and TPC
+//    points in it. Again, all is done automatically:
+//    
+//        Bool_t AddTrackPointArray( AliTrackPointArray* pTrackPointArr );
+//    
+//    For cosmic data, the assumption is that the tracking is done separately
+//    for ITS and TPC and the tracklets are saved inside one AliESDEvent. The
+//    method
+//    
+//        Bool_t AddCosmicEventSeparateTracking( AliESDEvent* pEvent );
+//    
+//    then searches the event for matching tracklets and upon succes it updates
+//    the fit. For cosmics the fitter switches to a 3 tracks mode, the 3 tracks
+//    being: 1 ITS and 2 TPC tracklets (upper and lower). 
+//    
+//    Experts only:
+//    
+//
+//    Origin: Mikolaj Krzewicki, Nikhef, Mikolaj.Krzewicki@cern.ch
+//
+//////////////////////////////////////////////////////////////////////////////
+
+#include "AliRelAlignerKalman.h"
+
+ClassImp(AliRelAlignerKalman)
+
+//########################################################
+//
+//    Control section
+//
+//########################################################
+
+AliRelAlignerKalman::AliRelAlignerKalman():
+    fPTrackPointArray1(new AliTrackPointArray(1)),
+    fPTrackPointArray2(new AliTrackPointArray(1)),
+    fPTrackPointArray2b(new AliTrackPointArray(1)),
+    fPVolIDArray1(new TArrayI(1)),
+    fPVolIDArray2(new TArrayI(1)),
+    fPVolIDArray2b(new TArrayI(1)),
+    fQ(1e-10),
+    fNMeasurementParams(fgkNMeasurementParams2TrackMode),
+    fNSystemParams(fgkNSystemParams),
+    fOutRejSigmas(2.),
+    f3TracksMode(kFALSE),
+    fSortTrackPointsWithY(kTRUE),
+    fFixedMeasurementCovariance(kTRUE),
+    fCalibrationPass(kFALSE),
+    fCalibrationUpdated(kFALSE),
+    fFitted(kFALSE),
+    fCuts(kFALSE),
+    fNTracks(0),
+    fNUpdates(0),
+    fNOutliers(0),
+    fNUnfitted(0),
+    fNMatchedCosmics(0),
+    fMinPointsVol1(2),
+    fMinPointsVol2(10),
+    fMinMom(0.),
+    fMaxMom(1e100),
+    fMinAbsSinPhi(0.),
+    fMaxAbsSinPhi(1.),
+    fMinSinTheta(-1.),
+    fMaxSinTheta(1.),
+    fMaxMatchingAngle(0.1),
+    fMaxMatchingDistance(1.),  //in cm
+    fFirstLayerVol1(1),
+    fLastLayerVol1(6),
+    fFirstLayerVol2(7),
+    fLastLayerVol2(8)
+{
+    //Default constructor
+    
+    fPDir1 = new TVector3; //Direction
+    fPDir2 = new TVector3;
+    fPDir2b = new TVector3;
+    fPDir1Cov = new TMatrixDSym(3);
+    fPDir2Cov = new TMatrixDSym(3);
+    fPDir2bCov = new TMatrixDSym(3);
+    fPDir1ThetaPhiCov = new TMatrixDSym(2); //Direction vectors are unit vectors, therefore 2 independent components!
+    fPDir2ThetaPhiCov = new TMatrixDSym(2);
+    fPDir2bThetaPhiCov = new TMatrixDSym(2);
+    fPPoint1 = new TVector3;
+    fPPoint2 = new TVector3;
+    fPPoint2b = new TVector3;
+    fPPoint1Cov = new TMatrixDSym(3);
+    fPPoint2Cov = new TMatrixDSym(3);
+    fPPoint2bCov = new TMatrixDSym(3);
+    fPRefPoint = new TVector3(0.,0.,0.);
+    fPRefPointDir = new TVector3(0.,1.,0.);
+    Float_t refpointcov[6] = { 1., 0., 0., 
+                                   0., 0.,
+                                       1.  };
+    fPRefAliTrackPoint = new AliTrackPoint( 0.,0.,0.,refpointcov,0 );
+    fPMeasurement = new TVectorD( fNMeasurementParams );
+    fPMeasurementCov = new TMatrixDSym( fNMeasurementParams );
+    fPX = new TVectorD( fNSystemParams );
+    fPXcov = new TMatrixDSym( fNSystemParams );
+    fPH = new TMatrixD( fNMeasurementParams, fNSystemParams );
+    fPRotMat = new TMatrixD(3,3);
+    fPVec010 = new TVector3(0,1,0);
+
+    //default seed: zero, unit(large) errors
+    fPXcov->UnitMatrix();
+    (*fPXcov) *= 1.;
+
+    fPPhiMesHist = new TH1D("phi","phi", 50, 0, 0);
+    fPThetaMesHist = new TH1D("theta","theta", 50, 0, 0);
+    fPXMesHist = new TH1D("x","x", 50, 0, 0);
+    fPZMesHist = new TH1D("z","z", 50, 0, 0);
+    fPPhiMesHist2 = new TH1D("phi_","phi_", 50, 0, 0);
+    fPThetaMesHist2 = new TH1D("theta_","theta_", 50, 0, 0);
+    fPXMesHist2 = new TH1D("x_","x_", 50, 0, 0);
+    fPZMesHist2 = new TH1D("z_","z_", 50, 0, 0);
+}
+
+Bool_t AliRelAlignerKalman::AddESDTrack( AliESDtrack* pTrack )
+{
+    //Add an ESD track from a central event
+    //from the ESDtrack extract the pointarray
+
+    SetCosmicEvent(kFALSE);
+    
+    const AliTrackPointArray *pTrackPointArrayConst = pTrack->GetTrackPointArray();
+    AliTrackPointArray* pTrackPointArray = const_cast < AliTrackPointArray* > (pTrackPointArrayConst);    
+    if (pTrackPointArray == 0) return kFALSE;
+
+    if (!AddTrackPointArray( pTrackPointArray )) return kFALSE;
+    return kTRUE;
+}
+
+Bool_t AliRelAlignerKalman::AddTrackPointArray( AliTrackPointArray* pTrackPointArray )
+{
+    //Add a trackpointarray from a central event
+    
+    SetCosmicEvent(kFALSE);
+    fNTracks++; 
+
+    if (!ExtractSubTrackPointArray( fPTrackPointArray1, fPVolIDArray1, pTrackPointArray, 1,6 ))
+        return kFALSE;
+    if (!ExtractSubTrackPointArray( fPTrackPointArray2, fPVolIDArray2, pTrackPointArray, 7,8 ))
+        return kFALSE;
+   
+    if (!ProcessTrackPointArrays()) return kFALSE; 
+    if (!PrepareUpdate()) return kFALSE;
+    if (!Update()) return kFALSE;
+    
+    return kTRUE;
+}
+
+Bool_t AliRelAlignerKalman::AddCosmicEventSeparateTracking( AliESDEvent* pEvent )
+{
+    //Add an cosmic with separately tracked ITS and TPC parts, do trackmatching
+
+    SetCosmicEvent(kTRUE);  //set the cosmic flag
+    fNTracks++; 
+
+    if (!FindCosmicInEvent( pEvent )) return kFALSE;
+    if (!ProcessTrackPointArrays()) return kFALSE; 
+    //PrintDebugInfo();
+    if (!PrepareUpdate()) return kFALSE;
+    if (!Update()) return kFALSE;
+    printf("fpMeasurementCov:\n");
+    fPMeasurementCov->Print();
+
+    return kTRUE;
+}
+
+void AliRelAlignerKalman::Print()
+{
+    //Print some useful info
+    printf("\nAliRelAlignerKalman:\n");
+    printf("  %i tracks, %i updates, %i outliers, ", fNTracks, fNUpdates, fNOutliers );
+    printf("%i not fitted, %i matched cosmic tracklets\n", fNUnfitted, fNMatchedCosmics );
+    printf("  psi(x):         %.1f ± (%.1f) mrad\n", 1e3*(*fPX)(0),1e3*TMath::Sqrt((*fPXcov)(0,0)));
+    printf("  theta(y):       %.1f ± (%.1f) mrad\n", 1e3*(*fPX)(1),1e3*TMath::Sqrt((*fPXcov)(1,1)));
+    printf("  phi(z):         %.1f ± (%.1f) mrad\n", 1e3*(*fPX)(2),1e3*TMath::Sqrt((*fPXcov)(2,2)));
+    printf("  x:              %.1f ± (%.1f) micron\n", 1e4*(*fPX)(3),1e4*TMath::Sqrt((*fPXcov)(3,3)));
+    printf("  y:              %.1f ± (%.1f) micron\n", 1e4*(*fPX)(4),1e4*TMath::Sqrt((*fPXcov)(4,4)));
+    printf("  z:              %.1f ± (%.1f) micron\n", 1e4*(*fPX)(5),1e4*TMath::Sqrt((*fPXcov)(5,5)));
+    printf("  TPC(drift corr) %.1f ± (%.1f) percent\n", 1e2*(*fPX)(6),1e2*TMath::Sqrt((*fPXcov)(6,6)));
+    printf("  TPC(offset)     %.1f ± (%.1f) micron\n", 1e4*(*fPX)(7),1e4*TMath::Sqrt((*fPXcov)(7,7)));
+    return;
+}
+
+void AliRelAlignerKalman::SetTrackParams1( const TVector3& point, const TMatrixDSym& pointcov, const TVector3& dir, const TMatrixDSym& dircov )
+{
+    //Set the trackparameters for track in the first volume at reference
+    *fPPoint1 = point;
+    *fPPoint1Cov = pointcov;
+    *fPDir1 = dir;
+    *fPDir1Cov = dircov;
+}
+
+void AliRelAlignerKalman::SetTrackParams2( const TVector3& point, const TMatrixDSym& pointcov, const TVector3& dir, const TMatrixDSym& dircov )
+{
+    //Set the trackparameters for track in the second volume at reference
+    *fPPoint2 = point;
+    *fPPoint2Cov = pointcov;
+    *fPDir2 = dir;
+    *fPDir2Cov = dircov;
+}
+
+void AliRelAlignerKalman::SetTrackParams2b( const TVector3& point, const TMatrixDSym& pointcov, const TVector3& dir, const TMatrixDSym& dircov )
+{
+    //Set the trackparameters for second track in the second volume at reference
+    *fPPoint2b = point;
+    *fPPoint2bCov = pointcov;
+    *fPDir2b = dir;
+    *fPDir2bCov = dircov;
+}   
+
+void AliRelAlignerKalman::SetRefSurface( const Double_t yref )
+{
+    //set the reference surface
+    fPRefPoint->SetXYZ( 0., 1.*yref, 0. );
+    fPRefPointDir->SetXYZ( 0., 1., 0. );
+    Float_t refpointcov[6] = { 1., 0., 0., 
+                                   0., 0.,
+                                       1.  };
+    fPRefAliTrackPoint->SetXYZ( 0., 1.*yref, 0., refpointcov );
+}
+
+void AliRelAlignerKalman::SetRefSurface( const TVector3& point, const TVector3& dir )
+{
+    //set the reference surface
+    *fPRefPoint = point;
+    *fPRefPointDir = dir;
+}
+
+void AliRelAlignerKalman::SetRefSurface( const AliTrackPoint& point, const Bool_t horizontal )
+{
+    //set the reference surface
+    (*fPRefAliTrackPoint) = point;
+
+    (*fPRefPoint)(0) = point.GetX();
+    (*fPRefPoint)(1) = point.GetY();
+    (*fPRefPoint)(2) = point.GetZ();
+    
+    if (horizontal)
+    {
+        (*fPRefPointDir)(0) = 0.;
+        (*fPRefPointDir)(1) = 1.;
+        (*fPRefPointDir)(2) = 0.;
+        
+        Float_t refpointcov[6] = { 1., 0., 0., 
+                                       0., 0.,
+                                           1.  };
+        fPRefAliTrackPoint->SetXYZ( point.GetX(), point.GetY() , point.GetZ(), refpointcov );
+
+    }
+    else
+    {
+        Double_t angle = point.GetAngle();
+        (*fPRefPointDir)(0) = TMath::Cos(angle);
+        (*fPRefPointDir)(1) = TMath::Sin(angle);
+        (*fPRefPointDir)(2) = 0.;
+        *fPRefPointDir = fPRefPointDir->Unit();
+    }
+}
+
+void AliRelAlignerKalman::Set3TracksMode( Bool_t mode )
+{
+    //set the mode where 2 tracklets are allowed in volume 2 (default:TPC)
+    //used for alignment with cosmics
+
+    f3TracksMode = mode;
+    if (mode)
+    {
+        if (fNMeasurementParams!=fgkNMeasurementParams3TrackMode) //do something only if necessary
+        {
+            fNMeasurementParams = fgkNMeasurementParams3TrackMode;
+            delete fPMeasurement;
+            delete fPMeasurementCov;
+            delete fPH;
+            fPMeasurement = new TVectorD( fNMeasurementParams );
+            fPMeasurementCov = new TMatrixDSym( fNMeasurementParams );
+            fPH = new TMatrixD( fNMeasurementParams, fNSystemParams );
+        }
+    }
+    else
+    {
+        if (fNMeasurementParams!=fgkNMeasurementParams2TrackMode)
+        {
+            fNMeasurementParams = fgkNMeasurementParams2TrackMode;
+            delete fPMeasurement;
+            delete fPMeasurementCov;
+            delete fPH;
+            fPMeasurement = new TVectorD( fNMeasurementParams );
+            fPMeasurementCov = new TMatrixDSym( fNMeasurementParams );
+            fPH = new TMatrixD( fNMeasurementParams, fNSystemParams );
+        }
+    }
+}
+    
+Bool_t AliRelAlignerKalman::PrepareUpdate()
+{
+    //Cast the extrapolated data (points and directions) into
+    //the internal Kalman filter data representation.
+    //takes the 3d coordinates of the points of intersection with
+    //the reference surface and projects them onto a 2D plane.
+    //does the same for angles, combines the result in one vector
+
+    if (!FillMeasurement()) return kFALSE;
+    if (!FillMeasurementMatrix()) return kFALSE;
+    return kTRUE;
+}
+
+Bool_t AliRelAlignerKalman::Update()
+{
+    //perform the update - either kalman or calibration
+    if (fCalibrationPass) return UpdateCalibration();
+    else return UpdateEstimateKalman();
+}
+
+void AliRelAlignerKalman::RotMat( TMatrixD &R, const TVectorD& angles )
+{
+//Get Rotation matrix R given the Cardan angles psi, theta, phi (around x, y, z).
+    Double_t sinpsi = TMath::Sin(angles(0));
+    Double_t sintheta = TMath::Sin(angles(1));
+    Double_t sinphi = TMath::Sin(angles(2));
+    Double_t cospsi = TMath::Cos(angles(0));
+    Double_t costheta = TMath::Cos(angles(1));
+    Double_t cosphi = TMath::Cos(angles(2));
+    
+    R(0,0) = costheta*cosphi;
+    R(0,1) = -costheta*sinphi;
+    R(0,2) = sintheta;
+    R(1,0) = sinpsi*sintheta*cosphi + cospsi*sinphi;
+    R(1,1) = -sinpsi*sintheta*sinphi + cospsi*cosphi;
+    R(1,2) = -costheta*sinpsi;
+    R(2,0) = -cospsi*sintheta*cosphi + sinpsi*sinphi;
+    R(2,1) = cospsi*sintheta*sinphi + sinpsi*cosphi;
+    R(2,2) = costheta*cospsi;
+    return;
+}
+
+Bool_t AliRelAlignerKalman::FillMeasurement()
+{
+    //Take the track parameters and calculate the input to the Kalman filter
+
+    //Direction vectors 
+    //Measured is the difference between the polar angles - convert to that
+    (*fPMeasurement)(0) = fPDir2->Theta() - fPDir1->Theta();
+    (*fPMeasurement)(1) = fPDir2->Phi() - fPDir1->Phi();
+    
+    //Points
+    //Measured is the distance in XZ plane at Y of the reference point
+    (*fPMeasurement)(2) = fPPoint2->X() - fPPoint1->X();
+    (*fPMeasurement)(3) = fPPoint2->Z() - fPPoint1->Z();
+
+    //if 3 track mode set, set also the stuff for 2nd TPC track
+    if (f3TracksMode)
+    {
+        (*fPMeasurement)(4) = fPDir2b->Theta() - fPDir1->Theta();
+        (*fPMeasurement)(5) = fPDir2b->Phi() - fPDir1->Phi();
+        (*fPMeasurement)(6) = fPPoint2b->X() - fPPoint1->X();
+        (*fPMeasurement)(7) = fPPoint2b->Z() - fPPoint1->Z();
+    }
+
+    //Fill the covariance
+    if (fFixedMeasurementCovariance) return kTRUE;
+    //the dirs
+    GetThetaPhiCov( *fPDir1ThetaPhiCov, *fPDir1, *fPDir1Cov );
+    GetThetaPhiCov( *fPDir2ThetaPhiCov, *fPDir2, *fPDir2Cov );
+    fPMeasurementCov->SetSub( 0, (*fPDir1ThetaPhiCov) + (*fPDir2ThetaPhiCov) );
+    
+    //the points
+    (*fPMeasurementCov)(2,2) = (*fPPoint1Cov)(0,0)+(*fPPoint2Cov)(0,0);
+    (*fPMeasurementCov)(2,3) = (*fPPoint1Cov)(0,2)+(*fPPoint2Cov)(0,2);
+    (*fPMeasurementCov)(3,2) = (*fPPoint1Cov)(2,0)+(*fPPoint2Cov)(2,0);
+    (*fPMeasurementCov)(3,3) = (*fPPoint1Cov)(2,2)+(*fPPoint2Cov)(2,2);
+
+    //if 3 track mode set, set also the stuff for 2nd TPC track
+    if (f3TracksMode)
+    {
+        GetThetaPhiCov( *fPDir2bThetaPhiCov, *fPDir2b, *fPDir2bCov );
+        fPMeasurementCov->SetSub( 4, (*fPDir1ThetaPhiCov) + (*fPDir2bThetaPhiCov) );
+        (*fPMeasurementCov)(6,6) = (*fPPoint1Cov)(0,0)+(*fPPoint2bCov)(0,0);
+        (*fPMeasurementCov)(6,7) = (*fPPoint1Cov)(0,2)+(*fPPoint2bCov)(0,2);
+        (*fPMeasurementCov)(7,6) = (*fPPoint1Cov)(2,0)+(*fPPoint2bCov)(2,0);
+        (*fPMeasurementCov)(7,7) = (*fPPoint1Cov)(2,2)+(*fPPoint2bCov)(2,2);
+    }
+    return kTRUE;
+}
+
+Bool_t AliRelAlignerKalman::FillMeasurementMatrix()
+{
+    //Calculate the system matrix for the Kalman filter
+    //approximate the system using as reference the track in the first volume
+
+    Double_t delta = 1.e-8;
+    TVectorD z1( fNMeasurementParams );
+    TVectorD z2( fNMeasurementParams );
+    TVectorD x1( *fPX );
+    TVectorD x2( *fPX );
+    TMatrixD D( fNMeasurementParams, 1 );
+    for ( Int_t i=0; i<fNSystemParams; i++ )
+    {
+        x1 = *fPX;
+        x2 = *fPX;
+        x1(i) -= delta;
+        x2(i) += delta;
+        if (!PredictMeasurement( z1, x1 )) return kFALSE;
+        if (!PredictMeasurement( z2, x2 )) return kFALSE;
+        for (Int_t j=0; j<fNMeasurementParams; j++ )
+            D.GetMatrixArray()[j] = (z2.GetMatrixArray()[j]-z1.GetMatrixArray()[j])/(2.*delta);
+        fPH->SetSub( 0, i, D );
+    }
+    return kTRUE;
+}
+
+Bool_t AliRelAlignerKalman::PredictMeasurement( TVectorD& pred, const TVectorD& state )
+{
+    // Implements a system model for the Kalman fit
+    // pred is [dtheta, dphi, dx, dz, [dtheta', dphi', dx', dz'] ] - second part is for 2nd TPC track in cosmics
+    // state is [psi,theta,phi,x,y,z,driftTPC,offsetTPC]
+
+    TVector3 newPoint = *fPPoint1;
+    TVector3 newDir = *fPDir1;
+    TVector3 shift;
+    //TPC drift correction
+    newDir(2) *= (1.+state(6));
+    newPoint(2) *= (1.+state(6));
+    //TPC offset
+    if (newPoint(2)>0) newPoint(2) += state(7);
+    else newPoint(2) -= state(7);
+    //rotate
+    TMatrixD rotmat(3,3);
+    RotMat( rotmat, state );
+    newDir = rotmat * newDir;
+    newPoint = rotmat * newPoint;
+    //shift
+    shift(0) = state(3);
+    shift(1) = state(4);
+    shift(2) = state(5);
+    newPoint += shift;
+    //Predicted measurement
+    if (!IntersectionLinePlane( newPoint, newPoint, newDir, *fPRefPoint, *fPVec010 )) return kFALSE;
+    pred(0) = newDir.Theta() - fPDir1->Theta();
+    pred(1) = newDir.Phi() - fPDir1->Phi();
+    pred(2) = newPoint(0) - fPPoint1->X();
+    pred(3) = newPoint(2) - fPPoint1->Z();
+
+    if (f3TracksMode)
+    {
+        //Do the same for second TPC track
+        //should be the same as the first TPC track
+        pred(4) = pred(0);
+        pred(5) = pred(1);
+        pred(6) = pred(2);
+        pred(7) = pred(3);
+    }
+    return kTRUE;
+}
+
+Bool_t AliRelAlignerKalman::UpdateEstimateKalman()
+{
+    //Kalman estimation of noisy constants: in the model A=1
+    //The arguments are (following the usual convention):
+    //  x - the state vector (parameters)
+    //  P - the state covariance matrix (parameter errors)
+    //  z - measurement vector
+    //  R - measurement covariance matrix
+    //  H - measurement model matrix ( z = Hx + v ) v being measurement noise (error fR)
+    TVectorD *x = fPX;
+    TMatrixDSym *P = fPXcov;
+    TVectorD *z = fPMeasurement;
+    TMatrixDSym *R = fPMeasurementCov;
+    TMatrixD *H = fPH;
+    
+    TMatrixDSym I(TMatrixDSym::kUnit, *P);            //unit matrix
+        
+    //predict the state
+    *P = *P + fQ*I;  //add some process noise (diagonal)
+    
+    // update prediction with measurement
+        // calculate Kalman gain
+        // K = PH/(HPH+R)
+    TMatrixD PHT( *P, TMatrixD::kMultTranspose, *H );  //common factor (used twice)
+    TMatrixD HPHT( *H, TMatrixD::kMult, PHT );
+    HPHT += *R;
+    TMatrixD K(PHT, TMatrixD::kMult, HPHT.Invert());                 //compute K
+  
+        // update the state and its covariance matrix
+    TVectorD xupdate(*x);
+    TVectorD Hx(*z);
+    PredictMeasurement( Hx, *x );
+    xupdate = K*((*z)-Hx);
+    
+    //SIMPLE OUTLIER REJECTION
+    if (
+            (TMath::Abs(xupdate(0)) > fOutRejSigmas*TMath::Sqrt((*P)(0,0))) ||
+            (TMath::Abs(xupdate(1)) > fOutRejSigmas*TMath::Sqrt((*P)(1,1))) ||
+            (TMath::Abs(xupdate(2)) > fOutRejSigmas*TMath::Sqrt((*P)(2,2))) ||
+            (TMath::Abs(xupdate(3)) > fOutRejSigmas*TMath::Sqrt((*P)(3,3))) ||
+            (TMath::Abs(xupdate(4)) > fOutRejSigmas*TMath::Sqrt((*P)(4,4))) ||
+            (TMath::Abs(xupdate(5)) > fOutRejSigmas*TMath::Sqrt((*P)(5,5))) ||
+            (TMath::Abs(xupdate(6)) > fOutRejSigmas*TMath::Sqrt((*P)(6,6))) ||
+            (TMath::Abs(xupdate(7)) > fOutRejSigmas*TMath::Sqrt((*P)(7,7))) 
+        )
+    {
+        fNOutliers++;
+        return kFALSE;
+    }
+    
+    *x += xupdate;
+    TMatrixD KH( K, TMatrixD::kMult, *H );
+    TMatrixD IKH(I);
+    IKH = I - KH;
+    TMatrixD IKHP( IKH, TMatrixD::kMult, *P ); // (I-KH)P
+    TMatrixDSym_from_TMatrixD( *P, IKHP );
+    fNUpdates++;
+    return kTRUE;
+}
+
+void AliRelAlignerKalman::TMatrixDSym_from_TMatrixD( TMatrixDSym& matsym, const TMatrixD& mat )
+{
+    //Produce a valid symmetric matrix out of a TMatrixD
+
+    //not very efficient, diagonals are computer twice, but who cares
+    for (Int_t i=0; i<mat.GetNcols(); i++)
+    {
+        for (Int_t j=i; j<mat.GetNcols(); j++)
+        {
+            Double_t average = (mat(i,j)+mat(j,i))/2.;
+            matsym(i,j)=average;
+            matsym(j,i)=average;
+        }
+    }
+    return;
+}
+
+void AliRelAlignerKalman::GetThetaPhiCov( TMatrixDSym& cov, const TVector3& vec, const TMatrixDSym& vecCov )
+{
+    //Given the covariance matrix of a vector in euclid.space
+    //give cov matrix of the 2 spherical angles
+    const Double_t delta = 1e-8;
+    TVector3 vec1;
+    TVector3 vec2;
+    TMatrixD T(2,3);
+    for (Int_t i=0; i<3; i++)
+    {
+        vec1 = vec;
+        vec2 = vec;
+        vec1(i) -= delta/2.;
+        vec2(i) += delta/2.;
+        T(0,i) = (vec2.Theta() - vec1.Theta())/delta;
+        T(1,i) = (vec2.Phi() - vec1.Phi())/delta;
+    }
+    TMatrixD tmp( T, TMatrixD::kMult, vecCov );
+    TMatrixD nscov( tmp, TMatrixD::kMultTranspose, T );
+    cov(0,0) = nscov(0,0); cov(0,1) = nscov(0,1);
+    cov(1,0) = nscov(0,1); cov(1,1) = nscov(1,1);
+    return;
+}
+
+Bool_t AliRelAlignerKalman::IntersectionLinePlane( TVector3& intersection, const TVector3& linebase, const TVector3& linedir, const TVector3& planepoint, const TVector3& planenormal )
+{
+    //calculate the intersection point between a straight line and a plane
+    //plane is defined with a point in it and a normal, line has a point and direction
+    if (planenormal==TVector3(0,1,0))
+    {
+        if (linedir(1)==0) return kFALSE;
+        Double_t t = ( planepoint(1) - linebase(1) ) / linedir(1);
+        intersection = linebase + t*linedir;
+        return kTRUE;
+    }
+    else
+    {
+        Double_t dirdotn = linedir.Dot(planenormal);
+        if (dirdotn==0) return kFALSE;
+        Double_t t = ( planepoint.Dot(planenormal) - linebase.Dot(planenormal) ) / dirdotn;
+        intersection = linebase + t*linedir;
+        return kTRUE;
+    }
+}
+
+void AliRelAlignerKalman::Angles( TVectorD &angles, const TMatrixD &rotmat )
+{
+//Calculate the Cardan angles (psi,theta,phi) from rotation matrix
+//b = R*a 
+//
+        angles(0) = TMath::ATan2( -rotmat(1,2), rotmat(2,2) );
+        angles(1) = TMath::ASin( rotmat(0,2) );
+        angles(2) = TMath::ATan2( -rotmat(0,1), rotmat(0,0) );
+        return;
+}
+
+void AliRelAlignerKalman::PrintCorrelationMatrix()
+{
+    //Print the correlation matrix for the fitted parameters
+
+    for ( Int_t j=0; j<fNSystemParams; j++ )
+    {
+        for ( Int_t i=0; i<fNSystemParams; i++ )
+        {
+            printf("%1.2f  ", (*fPXcov)(i,j)/TMath::Sqrt( (*fPXcov)(i,i) * (*fPXcov)(j,j) ) );
+        }//for i
+        printf("\n");
+    }//for j
+    printf("\n");
+    return;
+}
+
+Bool_t AliRelAlignerKalman::FindCosmicInEvent( AliESDEvent* pEvent )
+{
+    //Find track point arrays belonging to one cosmic in a separately tracked ESD
+    //and put them in the apropriate data members
+
+    //Sanity cuts on tracks + check which tracks are ITS which are TPC
+    Int_t ntracks = pEvent->GetNumberOfTracks(); //printf("number of tracks in event: %i\n", ntracks);
+    if(ntracks<2) return kFALSE;
+    Float_t* phiArr = new Float_t[ntracks];
+    Float_t* thetaArr = new Float_t[ntracks];
+    Double_t* distanceFromVertexArr = new Double_t[ntracks];
+    Int_t* goodtracksArr = new Int_t[ntracks];
+    Int_t* ITStracksArr = new Int_t[ntracks];
+    Int_t* TPCtracksArr = new Int_t[ntracks];
+    Int_t* nPointsArr = new Int_t[ntracks];
+    Int_t nITStracks = 0;
+    Int_t nTPCtracks = 0;
+    Int_t nGoodTracks = 0;
+    AliESDtrack* pTrack = NULL;
+    
+    const AliESDVertex* pVertex = pEvent->GetVertex();
+    Double_t vertexposition[3];
+    pVertex->GetXYZ(vertexposition);
+    
+    Double_t magfield = pEvent->GetMagneticField();
+
+    //select sane tracks
+    for (Int_t itrack=0; itrack < ntracks; itrack++)
+    {
+        pTrack = pEvent->GetTrack(itrack);
+        if (!pTrack) {cout<<"no track!"<<endl;continue;}
+        if(pTrack->GetNcls(0)+pTrack->GetNcls(1) < fMinPointsVol1) continue;
+        Float_t phi = pTrack->GetAlpha()+TMath::ASin(pTrack->GetSnp());
+        Float_t theta = 0.5*TMath::Pi()-TMath::ATan(pTrack->GetTgl());
+        //printf("phi: %4.2f theta: %4.2f\n", phi, theta);
+        if(fCuts)
+        {
+            if(pTrack->GetP()<fMinMom || pTrack->GetP()>fMaxMom) continue;
+            Float_t abssinphi = TMath::Abs(TMath::Sin(phi));
+            if(abssinphi<fMinAbsSinPhi || abssinphi>fMaxAbsSinPhi) continue;
+            Float_t sintheta = TMath::Sin(theta);
+            if(sintheta<fMinSinTheta || sintheta>fMaxSinTheta) continue;
+        }
+        goodtracksArr[nGoodTracks]=itrack;
+        phiArr[nGoodTracks]=phi;
+        thetaArr[nGoodTracks]=theta;
+
+        pTrack->RelateToVertex( pVertex, magfield, 10000. );
+        Double_t trackposition[3];
+        pTrack->GetXYZ( trackposition );
+        distanceFromVertexArr[nGoodTracks] = 
+              TMath::Sqrt((trackposition[0]-vertexposition[0])*(trackposition[0]-vertexposition[0])
+                       + (trackposition[1]-vertexposition[1])*(trackposition[1]-vertexposition[1])
+                       + (trackposition[2]-vertexposition[2])*(trackposition[2]-vertexposition[2]));
+
+        //check if track is ITS or TPC
+        if ( ((pTrack->GetStatus()&AliESDtrack::kITSin)>0)&&
+             !((pTrack->GetStatus()&AliESDtrack::kTPCin)>0) )
+        {
+            ITStracksArr[nITStracks] = nGoodTracks;
+            nITStracks++;
+        }
+
+        if ( ((pTrack->GetStatus()&AliESDtrack::kTPCin)>0)&&
+             !((pTrack->GetStatus()&AliESDtrack::kITSin)>0) )
+        {
+            TPCtracksArr[nTPCtracks] = nGoodTracks;
+            nTPCtracks++;
+        }
+
+        nGoodTracks++;
+    }//for itrack   -   sanity cuts
+
+    //printf("there are good tracks, %d in ITS and %d TPC.\n", nITStracks, nTPCtracks);
+    
+    if( nITStracks < 2 || nTPCtracks < 2 )
+    {
+        delete [] goodtracksArr; goodtracksArr=0;
+        delete [] ITStracksArr; ITStracksArr=0;
+        delete [] TPCtracksArr; TPCtracksArr=0;
+        delete [] nPointsArr; nPointsArr=0;
+        delete [] phiArr; phiArr=0;
+        delete [] thetaArr; thetaArr=0;
+        delete [] distanceFromVertexArr; distanceFromVertexArr=0;
+        return kFALSE;
+    }
+
+    //find matching in TPC
+    Float_t min = 10000000.;
+    Int_t TPCgood1 = -1, TPCgood2 = -1;
+    for(Int_t itr1=0; itr1<nTPCtracks; itr1++)
+    {
+        for(Int_t itr2=itr1+1; itr2<nTPCtracks; itr2++)
+        {
+            Float_t deltatheta = TMath::Abs(TMath::Pi()-thetaArr[TPCtracksArr[itr1]]-thetaArr[TPCtracksArr[itr2]]);
+            if(deltatheta > fMaxMatchingAngle) continue;
+            Float_t deltaphi = TMath::Abs(TMath::Abs(phiArr[TPCtracksArr[itr1]]-phiArr[TPCtracksArr[itr2]])-TMath::Pi());
+            if(deltaphi > fMaxMatchingAngle) continue;
+            //printf("TPC: %f  %f     %f  %f\n",deltaphi,deltatheta,thetaArr[TPCtracksArr[itr1]],thetaArr[TPCtracksArr[itr2]]);
+            if(deltatheta+deltaphi<min) //only the best matching pair
+            {
+                min=deltatheta+deltaphi;
+                TPCgood1 = TPCtracksArr[itr1];  //store the index of track in goodtracksArr[]
+                TPCgood2 = TPCtracksArr[itr2];
+            }
+        }
+    }
+    if (TPCgood1 < 0) //no dubble cosmic track
+    {
+        delete [] goodtracksArr; goodtracksArr=0;
+        delete [] ITStracksArr; ITStracksArr=0;
+        delete [] TPCtracksArr; TPCtracksArr=0;
+        delete [] nPointsArr; nPointsArr=0;
+        delete [] phiArr; phiArr=0;
+        delete [] thetaArr; thetaArr=0;
+        delete [] distanceFromVertexArr; distanceFromVertexArr=0;
+        return kFALSE;
+    }
+
+    //in ITS find 2 tracks that best match the found TPC cosmic
+    Double_t min1 = 10000000.;
+    Double_t min2 = 10000000.;
+    Int_t ITSgood1 = -1, ITSgood2 = -1;
+    for(Int_t itr1=0; itr1<nITStracks; itr1++)
+    {
+        if(distanceFromVertexArr[ITStracksArr[itr1]]>fMaxMatchingDistance) continue;
+        Float_t deltatheta = TMath::Abs(TMath::Pi()-thetaArr[ITStracksArr[itr1]]-thetaArr[TPCgood1]);
+        if(deltatheta > fMaxMatchingAngle) deltatheta = TMath::Abs( deltatheta - TMath::Pi() );
+        if(deltatheta > fMaxMatchingAngle) continue;
+        Float_t deltaphi = TMath::Abs(TMath::Abs(phiArr[ITStracksArr[itr1]]-phiArr[TPCgood1])-TMath::Pi());
+        if(deltaphi > fMaxMatchingAngle) deltaphi = TMath::Abs( deltaphi - TMath::Pi() );
+        if(deltaphi > fMaxMatchingAngle) continue;
+        //printf("ITS %i dtheta, dphi, vrtx: %.4f, %.4f, %.4f\n",ITStracksArr[itr1], deltatheta, deltaphi,distanceFromVertexArr[ITStracksArr[itr1]]);
+        if(deltatheta+deltaphi<min1) //only the best one
+        {
+            min1=deltatheta+deltaphi;
+            //ITSgood2 = ITSgood1;
+            ITSgood1 = ITStracksArr[itr1];
+            continue;
+        }
+        if(deltatheta+deltaphi<min2) //the second best
+        {
+            min2=deltatheta+deltaphi;
+            ITSgood2 = ITStracksArr[itr1];
+            continue;
+        }
+    }
+    if (ITSgood2 < 0) //no ITS cosmic track
+    {
+        delete [] goodtracksArr; goodtracksArr=0;
+        delete [] ITStracksArr; ITStracksArr=0;
+        delete [] TPCtracksArr; TPCtracksArr=0;
+        delete [] nPointsArr; nPointsArr=0;
+        delete [] phiArr; phiArr=0;
+        delete [] thetaArr; thetaArr=0;
+        delete [] distanceFromVertexArr; distanceFromVertexArr=0;
+        return kFALSE;
+    }
+
+    //we found a cosmic
+    fNMatchedCosmics++;
+    
+    //////////////////////////////////////////////////////////////////////////////////////
+    // convert indexes from local goodtrackarrays to global track index
+    TPCgood1 = goodtracksArr[TPCgood1];
+    TPCgood2 = goodtracksArr[TPCgood2];
+    ITSgood1 = goodtracksArr[ITSgood1];
+    ITSgood2 = goodtracksArr[ITSgood2];
+    /////////////////////////////////////////////////////////////////////////////////////
+
+    AliESDtrack * pTPCtrack1 = pEvent->GetTrack(TPCgood1);
+    AliESDtrack * pTPCtrack2 = pEvent->GetTrack(TPCgood2);
+    AliESDtrack * pITStrack1 = pEvent->GetTrack(ITSgood1);
+    AliESDtrack * pITStrack2 = pEvent->GetTrack(ITSgood2);
+    const AliTrackPointArray* pTPCArray1 = pTPCtrack1->GetTrackPointArray();
+    const AliTrackPointArray* pTPCArray2 = pTPCtrack2->GetTrackPointArray();
+    const AliTrackPointArray* pITSArray1 = pITStrack1->GetTrackPointArray();
+    const AliTrackPointArray* pITSArray2 = pITStrack2->GetTrackPointArray();
+
+    AliTrackPointArray* pfullITStrackArray = new AliTrackPointArray(1);
+    JoinTrackArrays( pfullITStrackArray, pITSArray1, pITSArray2 );
+
+    //cout<<"selected tracks: TPC: "<<TPCgood1<<" "<<TPCgood2<<" ITS: "<<ITSgood1<<" "<<ITSgood2<<endl;
+
+    //Fill the final trackpointarrays
+    if (!ExtractSubTrackPointArray( fPTrackPointArray1,  fPVolIDArray1,  pfullITStrackArray, 1, 6 )) return kFALSE;
+    if (!ExtractSubTrackPointArray( fPTrackPointArray2,  fPVolIDArray2,  pTPCArray1,         7, 8 )) return kFALSE;
+    if (!ExtractSubTrackPointArray( fPTrackPointArray2b, fPVolIDArray2b, pTPCArray2,         7, 8 )) return kFALSE;
+
+    delete pfullITStrackArray; pfullITStrackArray=NULL;
+    delete [] goodtracksArr; goodtracksArr=NULL;
+    delete [] ITStracksArr; ITStracksArr=NULL;
+    delete [] TPCtracksArr; TPCtracksArr=NULL;
+    delete [] nPointsArr; nPointsArr=NULL;
+    delete [] phiArr; phiArr=NULL;
+    delete [] thetaArr; thetaArr=NULL;
+    delete [] distanceFromVertexArr; distanceFromVertexArr=NULL;
+    //printf("FindCosmicInEvent END\n");
+    return kTRUE;
+}
+
+Bool_t AliRelAlignerKalman::ExtractSubTrackPointArray( AliTrackPointArray* pTrackOut, TArrayI* pVolIDArrayOut, const AliTrackPointArray* pTrackIn, const Int_t firstlayer, const Int_t lastlayer )
+{
+    //printf("ExtractSubTrackPointArray\n");
+    //From pTrackIn select points between firstlayer and lastlayer and put the result in pTrackOut.
+    //VolID's of selected points go in pVolIDArrayOut
+    //only good points selected.
+    //points can be ordered with z by setting fSortTrackPointsWithY
+
+    AliTrackPoint point;
+    Int_t nPointsIn = pTrackIn->GetNPoints();
+    if (nPointsIn<TMath::Min(fMinPointsVol1,fMinPointsVol2)) return kFALSE;
+    Int_t iPointArr[1000];
+    Int_t VolIDArr[1000];
+    Float_t yArr[1000];
+    Int_t iOuter=-1;
+    Int_t OuterLayerID=0;
+    Int_t nGood=0;
+    
+    //loop over trackpoints and record the good ones
+    for (Int_t i=0; i<nPointsIn; i++)
+    {
+        UShort_t VolIDshort = pTrackIn->GetVolumeID()[i];
+        Int_t layerID = AliGeomManager::VolUIDToLayer(VolIDshort);
+        //some points are very dirty: have nan's in coordinates or are otherwise sick
+        {
+        Float_t x = pTrackIn->GetX()[i];
+        Float_t y = pTrackIn->GetY()[i];
+        Float_t z = pTrackIn->GetZ()[i];
+        if ( ((TMath::Abs(x)<1.)||(TMath::Abs(x)>1000.))&&
+             ((TMath::Abs(y)<1.)||(TMath::Abs(y)>1000.))||
+             ((TMath::Abs(z)>1000.))
+           )
+            continue;
+        }
+        if ( (layerID >= firstlayer) && (layerID <= lastlayer) )
+        {
+            
+            iPointArr[nGood] = i;
+            VolIDArr[nGood] = VolIDshort;
+            yArr[nGood] = pTrackIn->GetY()[i];
+            if (layerID>OuterLayerID)
+            {
+                OuterLayerID=layerID;
+                iOuter = nGood;
+            }
+            nGood++;
+        }
+        else continue;
+    }//for i=0..nPointsIn
+    if (nGood<TMath::Min(fMinPointsVol1,fMinPointsVol2)) return kFALSE;
+    //Fill the output array of VolID's
+    delete pVolIDArrayOut;
+    pVolIDArrayOut = new TArrayI( nGood );
+
+    //fill the output trackarray
+    Int_t* iSortedYArr = new Int_t[nGood];
+    if (fSortTrackPointsWithY)
+    {
+        TMath::Sort( nGood, yArr, iSortedYArr, kFALSE );
+    }
+    delete pTrackOut;
+    pTrackOut = new AliTrackPointArray( nGood );
+
+    for ( Int_t i=0; i<nGood; i++)
+    {
+        pTrackIn->GetPoint( point, iPointArr[(fSortTrackPointsWithY)?iSortedYArr[i]:i] );
+        pTrackOut->AddPoint( i, &point );
+        pVolIDArrayOut->AddAt( VolIDArr[(fSortTrackPointsWithY)?iSortedYArr[i]:i], i );
+    }
+    //printf("ExtractSubTrackPointArray END\n");
+    return kTRUE;
+}
+
+void AliRelAlignerKalman::SortTrackPointArrayWithY( AliTrackPointArray* pout, const AliTrackPointArray* pinn, const Bool_t descending )
+{
+    //sorts a given trackpointarray by y coordinate
+
+    Int_t npoints = pinn->GetNPoints();
+    const AliTrackPointArray* pin;
+    if (pinn==pout)
+        pin = new AliTrackPointArray(*pinn);
+    else
+    {
+        pin = pinn;
+        if (pout!=NULL) delete pout;
+        pout = new AliTrackPointArray(npoints);
+    }
+    
+    Int_t* iarr = new Int_t[npoints];
+
+    TMath::Sort( npoints, pin->GetY(), iarr, descending );
+
+    AliTrackPoint p;
+    for (Int_t i=0; i<npoints; i++)
+    {
+        pin->GetPoint( p, iarr[i] );
+        pout->AddPoint( i, &p );
+    }
+    delete [] iarr;
+}
+
+Bool_t AliRelAlignerKalman::FitTrackPointArrayRieman( TVector3* pPoint, TMatrixDSym* pPointCov, TVector3* pDir, TMatrixDSym* pDirCov, AliTrackPointArray* pTrackPointArray, TArrayI* pVolIDs, AliTrackPoint* pRefPoint )
+{
+    //printf("FitTrackPointArrayRieman\n");
+    //Use AliTrackFitterRieman to fit a track through given point array
+    
+    AliTrackFitterRieman fitter;
+    AliTrackPoint trackPoint;
+    const Float_t* pointcov;
+    ////here's an ugly hack://///////
+    //AliTrackPointArray* pTrackPointArray = const_cast < AliTrackPointArray* > (pTrackPointArrayIn);
+    /////////////////////////////////
+    fitter.SetTrackPointArray( pTrackPointArray, kFALSE );
+    if ( !fitter.Fit( pVolIDs ) ) return kFALSE;
+    if ( !fitter.GetPCA( *fPRefAliTrackPoint, trackPoint ) ) return kFALSE;
+    Double_t xPoint = trackPoint.GetX();
+    pPoint->SetXYZ( xPoint, trackPoint.GetY(), trackPoint.GetZ() );
+    pDir->SetXYZ( 1., fitter.GetDYat(xPoint), fitter.GetDZat(xPoint) );
+    *pDir = pDir->Unit();
+    //TODO cov of dir!
+    
+    pointcov = trackPoint.GetCov();
+    (*pPointCov)(0,0) = pointcov[0];(*pPointCov)(0,1) = pointcov[1];(*pPointCov)(0,2) = pointcov[2];
+    (*pPointCov)(1,0) = pointcov[1];(*pPointCov)(1,1) = pointcov[3];(*pPointCov)(1,2) = pointcov[4];
+    (*pPointCov)(2,0) = pointcov[2];(*pPointCov)(2,1) = pointcov[4];(*pPointCov)(2,2) = pointcov[5];
+
+    //printf("FitTrackPointArrayRieman END\n");
+    return kTRUE;
+}
+
+Bool_t AliRelAlignerKalman::FitTrackPointArrayKalman( TVector3* pPoint, TMatrixDSym* pPointCov, TVector3* pDir, TMatrixDSym* pDirCov, AliTrackPointArray* pTrackPointArray, TArrayI* pVolIDs, AliTrackPoint* pRefPoint )
+{
+    //printf("FitTrackPointArrayKalman\n");
+    //Use AliTrackFitterKalman to fit a track through given point array
+    //still needs work
+    
+    AliTrackFitterKalman fitter;
+    AliTrackPoint trackPoint, trackPoint2;
+
+    pTrackPointArray->GetPoint( trackPoint, 0 );
+    pTrackPointArray->GetPoint( trackPoint2, 1 );
+    //Make sure all points count
+    fitter.SetMaxChi2(100000000.);
+    if (!fitter.MakeSeed( &trackPoint, &trackPoint2 )) return kFALSE;
+    Int_t npoints = pTrackPointArray->GetNPoints();
+    for (Int_t i=2; i<npoints; i++)
+    {
+        pTrackPointArray->GetPoint( trackPoint, i );
+        if (!fitter.AddPoint( &trackPoint )) continue;
+    }//for i
+    TMatrixDSym fullcov = fitter.GetCovariance();
+    printf("FitTrackPointArrayKalman: the covariance matrix of the fit");
+    fullcov.Print();
+    
+    //now propagate to ref plane - its a hack that depends on the implementation of AddPoint()!
+    // - first set the maxchi2 to 0 so addpoint returns false, but
+    //actually the trackparameters have already been been propagated to the new planei, it's safe
+    //because AliTrackFitterKalman::Propagate() is always kTRUE.
+    fitter.SetMaxChi2(0.);
+    fitter.AddPoint( fPRefAliTrackPoint );
+        
+    const Double_t* pparams = fitter.GetParam();
+    fullcov = fitter.GetCovariance();
+
+    pDir->SetXYZ(  pparams[3], 1., pparams[4] );
+    pPoint->SetXYZ( pparams[0], pparams[1], pparams[2] );
+    
+    (*pPointCov)(0,0) = fullcov(0,0);(*pPointCov)(0,1) = fullcov(0,1);(*pPointCov)(0,2) = fullcov(0,2);
+    (*pPointCov)(1,0) = fullcov(1,0);(*pPointCov)(1,1) = fullcov(1,1);(*pPointCov)(1,2) = fullcov(1,2);
+    (*pPointCov)(2,0) = fullcov(2,0);(*pPointCov)(2,1) = fullcov(2,1);(*pPointCov)(2,2) = fullcov(2,2);
+
+    (*pDirCov)(0,0)=fullcov(3,3); (*pDirCov)(0,1)=0.; (*pDirCov)(0,2)=fullcov(3,4);
+    (*pDirCov)(1,0)=0.;           (*pDirCov)(1,1)=0.; (*pDirCov)(1,2)=0.;
+    (*pDirCov)(2,0)=fullcov(4,3); (*pDirCov)(2,1)=0.; (*pDirCov)(2,2)=fullcov(4,4);
+    return kTRUE;
+}
+
+void AliRelAlignerKalman::SanitizeExtendedCovMatrix( TMatrixDSym& covout, const TMatrixDSym& pointcov, const TVector3& dir )
+{
+    //reverse the effect of extending of the covariance matrix along the track
+    //direction as done by the AliTrackFitters
+
+    //idea is to rotate to a system where track direction is y axis,
+    //in that refsystem setting the y related components of covariance
+    //then rotate back.
+    TVector3 yaxis(0,1,0);
+    Double_t angle = dir.Angle(yaxis);
+    TVector3 rotaxisvec = dir.Cross(yaxis);
+    TVector3 rotaxis = rotaxisvec.Unit();
+    TMatrixD R(3,3);
+
+    //Fill the rotation matrix.
+    Double_t ex=rotaxis(0);
+    Double_t ey=rotaxis(1);
+    Double_t ez=rotaxis(2);
+    Double_t sin = TMath::Sin(angle);
+    Double_t cos = TMath::Cos(angle);
+    Double_t icos = 1 - cos;
+    R(0,0) = cos + (ex*ex)*icos;
+    R(0,1) = icos*ex*ey - ez*sin;
+    R(0,2) = icos*ex*ez + ey*sin;
+    R(1,0) = icos*ey*ex + ez*sin;
+    R(1,1) = cos + (ey*ey)*icos;
+    R(1,2) = icos*ey*ez - ex*sin;
+    R(2,0) = icos*ez*ex - ey*sin;
+    R(2,1) = icos*ez*ey + ex*sin;
+    R(2,2) = cos + (ez*ez)*icos;
+
+    //Transform covariance:
+    TMatrixD covR( pointcov, TMatrixD::kMultTranspose, R);
+    TMatrixD RcovR( R, TMatrixD::kMult, covR );
+    TMatrixD newcov(3,3);
+    newcov(0,0)=RcovR(0,0);
+    newcov(0,2)=RcovR(0,2);
+    newcov(2,0)=RcovR(2,0);
+    newcov(2,2)=RcovR(2,2);
+    newcov(1,1)=(newcov(0,0)+newcov(2,2))/2.;  //this is a bit insane!!
+    covR = newcov *R;
+    RcovR = R.T() * covR;
+    TMatrixDSym_from_TMatrixD( covout, RcovR );
+}
+
+void AliRelAlignerKalman::JoinTrackArrays( AliTrackPointArray* pout, const AliTrackPointArray* pin1, const AliTrackPointArray* pin2 )
+{
+    //Join two AliTrackPointArrays
+
+    Int_t np1 = pin1->GetNPoints();
+    Int_t np2 = pin2->GetNPoints();
+    if (pout!=NULL) delete pout;
+    pout = new AliTrackPointArray(np1+np2);
+    AliTrackPoint p;
+    for (Int_t i=0;i<np1;i++)
+    {
+        pin1->GetPoint( p, i );
+        pout->AddPoint( i, &p );
+    }
+    for (Int_t i=0;i<np2;i++)
+    {
+        pin2->GetPoint( p, i );
+        pout->AddPoint( i+np1, &p );
+    }
+}
+
+Bool_t AliRelAlignerKalman::ProcessTrackPointArrays()
+{
+    //Fit the track point arrays and update some household info
+    
+    fFitted=kFALSE; //not fitted yet
+    if ( !FitTrackPointArrayKalman( fPPoint2, fPPoint2Cov, fPDir2, fPDir2Cov,
+                                     fPTrackPointArray2, fPVolIDArray2, fPRefAliTrackPoint ) )
+    {
+        fNUnfitted++;
+        return kFALSE;
+    }
+
+    if ( f3TracksMode )
+    {
+        if ( !FitTrackPointArrayKalman( fPPoint2b, fPPoint2bCov, fPDir2b, fPDir2bCov,
+                                         fPTrackPointArray2b, fPVolIDArray2b, fPRefAliTrackPoint ) )
+        {
+            fNUnfitted++;
+            return kFALSE;
+        }
+    }
+
+    if ( !FitTrackPointArrayKalman( fPPoint1, fPPoint1Cov, fPDir1, fPDir1Cov,
+                                     fPTrackPointArray1, fPVolIDArray1, fPRefAliTrackPoint ) )
+    {
+        fNUnfitted++;
+        return kFALSE;
+    }
+
+    fFitted=kTRUE;
+    //printf("ProcessTrackPointArrays: fitted!\n");
+    return kTRUE;
+}
+
+Bool_t AliRelAlignerKalman::UpdateCalibration()
+{
+    //Update the calibration with new data, used in calibration pass
+
+    fPThetaMesHist->Fill( (*fPMeasurement)(0) );
+    fPPhiMesHist->Fill( (*fPMeasurement)(1) );
+    fPXMesHist->Fill( (*fPMeasurement)(2) );
+    fPZMesHist->Fill( (*fPMeasurement)(3) );
+    if (f3TracksMode)
+    {
+        fPThetaMesHist2->Fill( (*fPMeasurement)(4) );
+        fPPhiMesHist2->Fill( (*fPMeasurement)(5) );
+        fPXMesHist2->Fill( (*fPMeasurement)(6) );
+        fPZMesHist2->Fill( (*fPMeasurement)(7) );
+    }
+    fCalibrationUpdated=kTRUE;
+    return kTRUE;
+}
+
+Bool_t AliRelAlignerKalman::SetCalibrationPass( Bool_t cp )
+{
+    //sets the calibration mode
+    if (cp)
+    {
+        fCalibrationPass=kTRUE;
+        return kTRUE;
+    }//if (cp)
+    else
+    {
+        if (!fCalibrationUpdated) return kFALSE;
+        if (fCalibrationPass) // do it only after the calibration pass
+        {
+            Double_t tmp;
+            fPMeasurementCov->Zero(); //reset the covariance
+
+            fPThetaMesHist->Fit("gaus");
+            TF1* fitformula = fPThetaMesHist->GetFunction("gaus");
+            tmp = fitformula->GetParameter(2);
+            (*fPMeasurementCov)(0,0) = tmp*tmp;
+
+            fPPhiMesHist->Fit("gaus");
+            fitformula = fPPhiMesHist->GetFunction("gaus");
+            tmp = fitformula->GetParameter(2);
+            (*fPMeasurementCov)(1,1) = tmp*tmp;
+
+            fPXMesHist->Fit("gaus");
+            fitformula = fPXMesHist->GetFunction("gaus");
+            tmp = fitformula->GetParameter(2);
+            (*fPMeasurementCov)(2,2) = tmp*tmp;
+
+            fPZMesHist->Fit("gaus");
+            fitformula = fPZMesHist->GetFunction("gaus");
+            tmp = fitformula->GetParameter(2);
+            (*fPMeasurementCov)(3,3) = tmp*tmp;
+
+            if (f3TracksMode)
+            {
+                fPThetaMesHist2->Fit("gaus");
+                fitformula = fPThetaMesHist2->GetFunction("gaus");
+                tmp = fitformula->GetParameter(2);
+                (*fPMeasurementCov)(4,4) = tmp*tmp;
+
+                fPPhiMesHist2->Fit("gaus");
+                fitformula = fPPhiMesHist2->GetFunction("gaus");
+                tmp = fitformula->GetParameter(2);
+                (*fPMeasurementCov)(5,5) = tmp*tmp;
+
+                fPXMesHist2->Fit("gaus");
+                fitformula = fPXMesHist2->GetFunction("gaus");
+                tmp = fitformula->GetParameter(2);
+                (*fPMeasurementCov)(6,6) = tmp*tmp;
+
+                fPZMesHist2->Fit("gaus");
+                fitformula = fPZMesHist2->GetFunction("gaus");
+                tmp = fitformula->GetParameter(2);
+                (*fPMeasurementCov)(7,7) = tmp*tmp;
+            }
+
+            fCalibrationPass=kFALSE;
+            fFixedMeasurementCovariance=kTRUE;
+            fPMeasurementCov->Print();
+            return kTRUE;
+        }//if (fCalibrationPass)
+    return kFALSE;
+    }//else (cp)
+    return kFALSE;
+}
+
+void AliRelAlignerKalman::PrintDebugInfo()
+{
+    //prints debug info
+    cout<<"AliRelAlignerKalman debug info"<<endl;
+    printf("fPTrackPointArray1 y: ");
+    for (Int_t i=0; i<fPTrackPointArray1->GetNPoints();i++)
+    {
+        printf("%.2f ",fPTrackPointArray1->GetY()[i]);
+    }
+    printf("\n");
+    printf("fPTrackPointArray2 y: ");
+    for (Int_t i=0; i<fPTrackPointArray2->GetNPoints();i++)
+    {
+        printf("%.2f ",fPTrackPointArray2->GetY()[i]);
+    }
+    printf("\n");
+    printf("fPTrackPointArray2b y: ");
+    for (Int_t i=0; i<fPTrackPointArray2b->GetNPoints();i++)
+    {
+        printf("%.2f ",fPTrackPointArray2b->GetY()[i]);
+    }
+    printf("\n");
+
+    printf("Measurement covariance");
+    fPMeasurementCov->Print();
+}
+
diff --git a/STEER/AliRelAlignerKalman.h b/STEER/AliRelAlignerKalman.h
new file mode 100644 (file)
index 0000000..5c890d7
--- /dev/null
@@ -0,0 +1,212 @@
+#ifndef ALIRELALIGNERKALMAN_H
+#define ALIRELALIGNERKALMAN_H
+
+/* Copyright(c) 1998-1999, ALICE Experiment at CERN, All rights reserved. *
+ * See cxx source for full Copyright notice                               */
+
+///////////////////////////////////////////////////////////////////////////////
+//
+//     Relative alignment of two tracking volumes (default ITS and TPC)
+//     (see AliRelAlignerKalman.cxx for details)
+//
+//     Origin: Mikolaj Krzewicki, Nikhef, Mikolaj.Krzewicki@cern.ch
+//
+//////////////////////////////////////////////////////////////////////////////
+
+#include <iostream>
+#include "TMath.h"
+#include "TMatrixD.h"
+#include "TMatrixDSym.h"
+#include "TVectorD.h"
+#include "TVectorD.h"
+#include "TDecompLU.h"
+#include "TVector3.h"
+#include "TArrayI.h"
+
+#include "AliESDtrack.h"
+#include "AliTrackPointArray.h"
+#include "AliGeomManager.h"
+#include "AliTrackFitterKalman.h"
+#include "AliTrackFitterRieman.h"
+#include "AliESDfriendTrack.h"
+#include "AliESDEvent.h"
+#include "AliESDVertex.h"
+#include "TH1D.h"
+#include "TF1.h"
+
+class AliRelAlignerKalman {
+
+public:
+    AliRelAlignerKalman();
+
+    //User methods:
+    Bool_t AddESDTrack( AliESDtrack* pTrack );
+    Bool_t AddTrackPointArray( AliTrackPointArray* pTrackPointArr );
+    Bool_t AddCosmicEventSeparateTracking( AliESDEvent* pEvent );
+    
+    void Print();
+    void PrintCorrelationMatrix();
+
+    void GetMeasurementCov( TMatrixDSym& cov ) { cov = *fPMeasurementCov; }
+    void GetMeasurement( TVectorD& mes ) { mes = *fPMeasurement; }
+    void GetState( TVectorD& state ) { state = *fPX; }
+    void GetStateCov ( TMatrixDSym& cov ) { cov = *fPXcov; }
+    void GetSeed( TVectorD& seed, TMatrixDSym& seedCov ) { seed = *fPX; seedCov = *fPXcov; }
+    Double_t* GetStateArr() { return fPX->GetMatrixArray(); }
+    Double_t* GetStateCovArr() { return fPXcov->GetMatrixArray(); }
+    Double_t* GetMeasurementArr() { return fPMeasurement->GetMatrixArray(); }
+    Double_t* GetMeasurementCovArr() { return fPMeasurementCov->GetMatrixArray(); }
+    
+    Bool_t SetCalibrationPass( Bool_t cp=kTRUE );
+
+    //Experts only:
+    void SetOutRejSigma( const Double_t a=2. ) { fOutRejSigmas = a; }
+    void SetMeasurementCov( const TMatrixDSym& cov ) {*fPMeasurementCov = cov;}
+    void SetMeasurement( const TVectorD& mes ) {*fPMeasurement = mes;}
+    void SetState( const TVectorD& param ) {*fPX = param;}
+    void SetStateCov (const TMatrixDSym& cov ) {*fPXcov = cov;}
+    void SetSeed( const TVectorD& seed, const TMatrixDSym& seedCov ) {*fPX = seed; *fPXcov = seedCov; }
+    void SetTrackParams1( const TVector3& point, const TMatrixDSym& pointcov, const TVector3& dir, const TMatrixDSym& dircov );
+    void SetTrackParams2( const TVector3& point, const TMatrixDSym& pointcov, const TVector3& dir, const TMatrixDSym& dircov );
+    void SetTrackParams2b( const TVector3& point, const TMatrixDSym& pointcov, const TVector3& dir, const TMatrixDSym& dircov );
+    void SetRefSurface( const TVector3& point, const TVector3& dir );
+    void SetRefSurface( const AliTrackPoint& point, const Bool_t horizontal=kTRUE );
+    void SetRefSurface( const Double_t y );
+    Bool_t PrepareUpdate();
+    Bool_t Update();
+    Bool_t IsReady() {return fFitted;}
+    void SetQ( const Double_t Q = 1e-10 ) { fQ = Q; }
+    void Set3TracksMode( Bool_t mode=kTRUE );
+    void SetCosmicEvent( Bool_t cosmic=kTRUE ) {Set3TracksMode(cosmic);}
+    void PrintDebugInfo();
+    
+protected:
+    AliRelAlignerKalman& operator= (const AliRelAlignerKalman& aligner );
+    AliRelAlignerKalman(const AliRelAlignerKalman&);
+
+    Bool_t UpdateEstimateKalman();
+    Bool_t FillMeasurement();
+    Bool_t FillMeasurementMatrix();
+    Bool_t PredictMeasurement( TVectorD& z, const TVectorD& x );
+    void GetThetaPhiCov( TMatrixDSym& cov, const TVector3& vec, const TMatrixDSym& vecCov );
+    void Angles( TVectorD &angles, const TMatrixD &rotmat );
+    void RotMat( TMatrixD& R, const TVectorD& angles );
+    Bool_t IntersectionLinePlane( TVector3& intersection, const TVector3& base, const TVector3& dir, const TVector3& p, const TVector3& n );
+    void TMatrixDSym_from_TMatrixD( TMatrixDSym& matsym, const TMatrixD& mat );
+    void SanitizeExtendedCovMatrix( TMatrixDSym& covout, const TMatrixDSym& pointcov, const TVector3& dir );
+    Bool_t ProcessTrackPointArrays();
+
+    Bool_t ExtractSubTrackPointArray( AliTrackPointArray* pTrackOut, TArrayI* pVolIDArrayOut, const AliTrackPointArray* pTrackIn, const Int_t firstlayer, const Int_t lastlayer );
+    Bool_t FitTrackPointArrayRieman( TVector3* pPoint, TMatrixDSym* pPointCov, TVector3* pDir, TMatrixDSym* pDirCov, AliTrackPointArray* pTrackPointArray, TArrayI* pVolIDs, AliTrackPoint* pRefPoint );
+    Bool_t FitTrackPointArrayKalman( TVector3* pPoint, TMatrixDSym* pPointCov, TVector3* pDir, TMatrixDSym* pDirCov, AliTrackPointArray* pTrackPointArray, TArrayI* pVolIDs, AliTrackPoint* pRefPoint );
+    Bool_t FindCosmicInEvent( AliESDEvent* pEvent );
+    void SortTrackPointArrayWithY( AliTrackPointArray* pout, const AliTrackPointArray* pin, const Bool_t descending=kFALSE );
+    void JoinTrackArrays( AliTrackPointArray* pout, const AliTrackPointArray* pin1, const AliTrackPointArray* pin2 );
+    Bool_t UpdateCalibration();
+
+private:
+    AliTrackPointArray* fPTrackPointArray1;  //track point array in first volume (ITS)
+    AliTrackPointArray* fPTrackPointArray2;  //track point array in second volume (TPC)
+    AliTrackPointArray* fPTrackPointArray2b; //optionally second trackpoint array in second volume (TPC)
+    TArrayI* fPVolIDArray1;                 //array with VolID's of the points in the trackpointarray 
+    TArrayI* fPVolIDArray2;                 //array with VolID's of the points in the trackpointarray
+    TArrayI* fPVolIDArray2b;                //array with VolID's of the points in the trackpointarray
+    TVector3* fPDir1;  //track 1 direction
+    TVector3* fPDir2;  //track 2 direction
+    TVector3* fPDir2b; //track 2b direction (for cosmics)
+    TMatrixDSym* fPDir1Cov;     //Covariance matrix of dir vec track 1
+    TMatrixDSym* fPDir2Cov;     //Covariance matrix of dir vec track 2
+    TMatrixDSym* fPDir2bCov;    //Covariance matrix of dir vec track 2b
+    TMatrixDSym* fPDir1ThetaPhiCov; //Covariance matrix of the dir vector in spherical coord (angles only)
+    TMatrixDSym* fPDir2ThetaPhiCov; //Covariance matrix of the dir vector in spherical coord (angles only)
+    TMatrixDSym* fPDir2bThetaPhiCov; //Covariance matrix of the dir vector in spherical coord (angles only)
+    TVector3* fPPoint1;  //track 1 extrapolated point at reference surface
+    TVector3* fPPoint2;  //track 2 extrapolated point at reference surface
+    TVector3* fPPoint2b; //track 2b extrapol. (for cosmics)
+    TMatrixDSym* fPPoint1Cov; //covariance matrix of point 1
+    TMatrixDSym* fPPoint2Cov; //covariance matrix of point 2
+    TMatrixDSym* fPPoint2bCov; //covariance matrix of point 2b
+    TVector3* fPRefPoint;    //a point on the reference surface
+    TVector3* fPRefPointDir; //reference surfaces' direction vec
+    AliTrackPoint* fPRefAliTrackPoint; //reference point as an AliTrackPoint (orientation stored in cov matrix)
+    TMatrixD* fPRotMat; // system rotation matrix for estimated alignment angles
+    
+    //
+    //Kalman filter related stuff
+    //
+    TVectorD* fPX; //System (fit) parameters (phi, theta, psi, x, y, z, driftcorr, driftoffset )
+    TMatrixDSym* fPXcov; //covariance matrix of system parameters
+
+    TMatrixD* fPH;      //System measurement matrix
+    Double_t fQ;        //measure for system noise
+    
+    TVectorD* fPMeasurement; //the measurement vec for Kalman filter (theta,phi,x,z)
+    TMatrixDSym* fPMeasurementCov; //measurement vec cvariance
+
+    Int_t fNMeasurementParams; //how many numbers do we measure?
+    Int_t fNSystemParams;      //how many parameters do we fit?
+
+    Double_t fOutRejSigmas; //number of sigmas for outlier rejection
+    //
+    //
+    //
+
+    //Control and calibration histograms
+    TH1D* fPXMesHist;  //histo of x measurement
+    TH1D* fPZMesHist;  //histo of y measurement
+    TH1D* fPPhiMesHist; //histo of phi measurement
+    TH1D* fPThetaMesHist; //histo of theta measurement
+    TH1D* fPXMesHist2;  //histo of x measurement (3tracks mode)
+    TH1D* fPZMesHist2;  //histo of y measurement (3tracks mode)
+    TH1D* fPPhiMesHist2; //histo of phi measurement (3tracks mode)
+    TH1D* fPThetaMesHist2; //histo of theta measurement (3tracks mode)
+    TH1D* fPMesCov11Hist;  //histogram of the covariance of a fit parameter, used in calibration
+    TH1D* fPMesCov22Hist;  //histogram of the covariance of a fit parameter, used in calibration
+    TH1D* fPMesCov33Hist;  //histogram of the covariance of a fit parameter, used in calibration
+    TH1D* fPMesCov44Hist;  //histogram of the covariance of a fit parameter, used in calibration
+    TH1D* fPMesCov55Hist;  //histogram of the covariance of a fit parameter, used in calibration
+    TH1D* fPMesCov66Hist;  //histogram of the covariance of a fit parameter, used in calibration
+    TH1D* fPMesCov77Hist;  //histogram of the covariance of a fit parameter, used in calibration
+    TH1D* fPMesCov88Hist;  //histogram of the covariance of a fit parameter, used in calibration
+    TMatrixDSym* fPMeasurementCovCorr; //correction to be applied to the measurement covariance
+    
+    Bool_t f3TracksMode; //are we using 3 tracklets?
+    Bool_t fSortTrackPointsWithY; //whether to sort the points after processing
+    Bool_t fFixedMeasurementCovariance; //don't fiddle with measurement cov - supply it externally
+    Bool_t fCalibrationPass;
+    Bool_t fCalibrationUpdated;
+    
+    Bool_t fFitted; //did fitting finish without error?
+    Bool_t fCuts;    //track cuts?
+
+    Int_t fNTracks; //number of processed tracks
+    Int_t fNUpdates; //number of successful Kalman update
+    Int_t fNOutliers; //number of outliers
+    Int_t fNUnfitted; //number of trackpointarrays that were not fitted for some reason
+    Int_t fNMatchedCosmics; //number of matched tracklets for cosmics
+
+    Int_t fMinPointsVol1;   //mininum number of points in volume 1
+    Int_t fMinPointsVol2;   //mininum number of points in volume 2
+    Double_t fMinMom;       //min momentum of track for track cuts
+    Double_t fMaxMom;       //max momentum of track for track cuts
+    Double_t fMinAbsSinPhi; //more cuts
+    Double_t fMaxAbsSinPhi; //more cuts
+    Double_t fMinSinTheta;  //cuts
+    Double_t fMaxSinTheta;  //cuts
+    Double_t fMaxMatchingAngle; //cuts
+    Double_t fMaxMatchingDistance; //cuts
+    Int_t fFirstLayerVol1; //first layer of volume 1
+    Int_t fLastLayerVol1;  //last layer of volume 1
+    Int_t fFirstLayerVol2; //first layer of volume 2
+    Int_t fLastLayerVol2;  //last layer of volume 2
+
+    TVector3 * fPVec010; //vector pointing up
+
+    static const Int_t fgkNMeasurementParams2TrackMode = 4; //how many measurables in 2 track mode
+    static const Int_t fgkNMeasurementParams3TrackMode = 8; //how many measurables in 3 track mode
+    static const Int_t fgkNSystemParams = 8;                //how many fit parameters
+    
+    ClassDef(AliRelAlignerKalman,1)     //AliRelAlignerKalman class
+};
+
+#endif