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