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