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