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