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