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