1. Adding sorting of the Points in point array
[u/mrichter/AliRoot.git] / TPC / AliTPCkalmanFit.cxx
1 /*
2   marian.ivanov@cern.ch 
3   
4   AliTPCkalmanFit:
5
6   Kalman filter(s) for fitting of the tracks together with calibration/transformation 
7   parameters.
8
9   Correction/Transformation are currently described by set (TObjArray) of primitive 
10   correction/transformatio - AliTPCTransformation.  Currently we assume that transformation 
11   comute (in first order). AliTPCTransformation describe general non linear transformation.   
12   
13   Current calibration parameters and covariance stored (fCalibParam, fCalibCovar).
14   
15   Currenly only linear track model implemented.
16   Fits to be implemented:
17    0. Plane fitting (Laser CE)
18    1. Primary vertex fitting.
19    2. Propagation in magnetic field and fit of planes
20
21
22   
23   How to use it - see  AliTPCkalmanFit::Test function
24
25   Simple test (see AliTPCkalmanFit::Test)  
26   AliTPCTransformation::BuildBasicFormulas();
27   AliTPCkalmanFit *kalmanFit0 = AliTPCkalmanFit::Test(2000);
28   TFile f("kalmanfitTest.root");
29  
30   Transformation visualization:
31   Transforamtion can be visualized using TFn (TF1,TF2 ...) and using tree->Draw()
32   e.g:
33   kalmanFit0->SetInstance(kalmanFit0);   // 
34   kalmanFit0->InitTransformation();      //
35   TF2 fxRZdz("fxRZdz","sign(y)*1000*(AliTPCkalmanFit::SGetTPCDeltaXYZ(0,-1,x,0,y)-AliTPCkalmanFit::SGetTPCDeltaXYZ(0,-1,x,0,y-1))",85,245,-250,250);
36   fxRZdz->Draw("");
37
38
39
40 */
41
42 #include "TRandom.h"
43 #include "TMath.h"
44 #include "TBits.h"
45 #include "TFormula.h"
46 #include "TF1.h"
47 #include "TLinearFitter.h"
48 #include "TFile.h"
49 #include "THnSparse.h"
50 #include "TAxis.h"
51
52
53 #include "TTreeStream.h"
54 #include "AliTrackPointArray.h"
55 #include "AliLog.h"
56 #include "AliTPCTransformation.h"
57 #include "AliTPCkalmanFit.h"
58
59 ClassImp(AliTPCkalmanFit)
60
61 AliTPCkalmanFit* AliTPCkalmanFit::fgInstance = 0;
62
63 AliTPCkalmanFit::AliTPCkalmanFit():
64   TNamed(),
65   fCalibration(0),
66   fCalibParam(0),
67   fCalibCovar(0),
68   fLinearParam(0),
69   fLinearCovar(0),
70   fLastTimeStamp(-1),
71   fCurrentAlpha(0),  //! current rotation frame
72   fCA(0),            //! cosine of current angle
73   fSA(0)            //! sinus of current angle    
74 {
75   //
76   // Default constructor
77   //  
78   for (Int_t ihis=0; ihis<12; ihis++){
79     fLinearTrackDelta[ihis]=0;
80     fLinearTrackPull[ihis]=0;
81   }
82 }
83
84 void AliTPCkalmanFit::InitTransformation(){
85   //
86   // Initialize pointers to the transforamtion functions
87   //
88   //
89   Int_t ncalibs = fCalibration->GetEntries();
90   for (Int_t icalib=0;icalib<ncalibs; icalib++){
91     AliTPCTransformation * transform = (AliTPCTransformation *)fCalibration->At(icalib);
92     transform->Init();
93   }
94 }
95
96 void AliTPCkalmanFit::Add(const AliTPCkalmanFit * kalman){
97   //
98   //
99   //
100   Update(kalman);
101   for (Int_t i=0;i<12;i++){
102     if (fLinearTrackDelta[i] && kalman->fLinearTrackDelta[i]){
103       fLinearTrackDelta[i]->Add(kalman->fLinearTrackDelta[i]);
104     }
105     if (fLinearTrackPull[i] && kalman->fLinearTrackPull[i]){
106       fLinearTrackPull[i]->Add(kalman->fLinearTrackPull[i]);
107     }
108   }
109   
110 }
111
112
113 void AliTPCkalmanFit::Init(){
114   //
115   // Initialize parameter vector and covariance matrix
116   // To be called after initialization of all of the transformations
117   //
118   //
119   Int_t ncalibs = fCalibration->GetEntries();
120   fCalibParam = new TMatrixD(ncalibs,1);
121   fCalibCovar = new TMatrixD(ncalibs,ncalibs);
122   for (Int_t icalib=0;icalib<ncalibs; icalib++){
123     AliTPCTransformation * transform = (AliTPCTransformation *)fCalibration->At(icalib);
124     (*fCalibParam)(icalib,0) = transform->fParam;
125     for (Int_t jcalib=0;jcalib<ncalibs; jcalib++){
126       if (icalib!=jcalib) (*fCalibCovar)(icalib,jcalib)= 0;
127       if (icalib==jcalib) (*fCalibCovar)(icalib,jcalib) = transform->fSigma*transform->fSigma;    
128     }
129   }
130   //
131   // Build QA histograms
132   //
133   Double_t mpi = TMath::Pi();
134   //
135   //                    delta  alpha   y0     z0    ky   kz 
136   Int_t binsQA[6]    = {300,   36*4,   30,    25,   20,  20};
137   Double_t xminQA[6] = {-0.5,  -mpi,  -120, -250,   -1,  -1.};
138   Double_t xmaxQA[6] = { 0.5,   mpi,   120,  250,    1,   1.};  
139   TString  axisName[6]={"#Delta",
140                         "#alpha",
141                         "y_{0}",
142                         "z_{0}",
143                         "tan(#phi)",
144                         "tan(theta)"};
145   //
146   TString deltaName[12]={"#Delta_{y}(cm)",
147                          "100*#Delta_{#phi}(cm)",
148                          "100^{2}dy0^{2}/dx0^{2}(cm)",
149                          "100^{2}dy1^{2}/dx1^{2}(cm)",
150                          "#Delta_{z}(cm)",
151                          "100*#Delta_{#theta}(cm)",
152                          "100^{2}*dz0^{2}/dx0^{2}(cm)",
153                          "100^{2}*dz1^{2}/dx1^{2}(cm)",
154                          "RMSy_{0} (cm)",
155                          "RMSy_{1} (cm)",
156                          "RMSz_{0} (cm)",
157                          "RMSz_{1} (cm)"};
158
159   TString pullName[12]={"#Delta_{y}(unit)",
160                         "100*#Delta_{#phi}(unit)",
161                         "100^{2}dy0^{2}/dx0^{2}(unit)",
162                         "100^{2}dy1^{2}/dx1^{2}(unit)",
163                         "#Delta_{z}(unit)",
164                         "100*#Delta_{#theta}(unit)",
165                         "100^{2}*dz0^{2}/dx0^{2}(unit)",
166                         "100^{2}*dz1^{2}/dx1^{2}(unit)"
167                         "RMSy_{0} (cm)",
168                         "RMSy_{1} (cm)",
169                         "RMSz_{0} (cm)",
170                         "RMSz_{1} (cm)"};
171   //
172   //
173   //
174   for (Int_t ihis=0; ihis<12; ihis++){
175     fLinearTrackDelta[ihis]=0;
176     fLinearTrackPull[ihis]=0;
177     xminQA[0]=-0.5; xmaxQA[0] = 0.5; 
178     fLinearTrackDelta[ihis]  = new THnSparseS(deltaName[ihis],deltaName[ihis], 6, binsQA,xminQA, xmaxQA);
179     xminQA[0]=-10; xmaxQA[0]  = 10;     
180     fLinearTrackPull[ihis]   = new THnSparseS(pullName[ihis],pullName[ihis],   6, binsQA,xminQA, xmaxQA);
181     for (Int_t iaxis=1; iaxis<6; iaxis++){
182       fLinearTrackDelta[ihis]->GetAxis(iaxis)->SetName(axisName[iaxis]);
183       fLinearTrackDelta[ihis]->GetAxis(iaxis)->SetTitle(axisName[iaxis]);
184       fLinearTrackPull[ihis]->GetAxis(iaxis)->SetName(axisName[iaxis]);
185       fLinearTrackPull[ihis]->GetAxis(iaxis)->SetTitle(axisName[iaxis]);
186     }
187     fLinearTrackDelta[ihis]->GetAxis(0)->SetName(deltaName[ihis]);
188     fLinearTrackDelta[ihis]->GetAxis(0)->SetTitle(deltaName[ihis]);
189     fLinearTrackPull[ihis]->GetAxis(0)->SetName(deltaName[ihis]);
190     fLinearTrackPull[ihis]->GetAxis(0)->SetTitle(deltaName[ihis]);
191   }
192
193   
194 }
195
196 void AliTPCkalmanFit::SetStatus(const char * mask, Bool_t setOn, Bool_t isOr){
197   //
198   // 0. To activate all transforamtion call SetStatus(0,kTRUE)
199   // 1. To disable everything               SetStatus(0,kFALSE)
200   // 2. To activate/desactivate             SetStatus("xxx",kTRUE/kFALSE,kFALSE)
201   Int_t ncalibs = fCalibration->GetEntries();
202   if (mask==0) {
203     for (Int_t i=0; i<ncalibs;i++){
204       AliTPCTransformation * transform = (AliTPCTransformation *)fCalibration->At(i);
205       transform->SetActive(setOn); 
206     }
207   }
208   else{
209     for (Int_t i=0; i<ncalibs;i++){
210       AliTPCTransformation * transform = (AliTPCTransformation *)fCalibration->At(i);
211       TString  strName(transform->GetName());
212       if (strName.Contains(mask)){
213         if (!isOr) transform->SetActive( transform->IsActive() && setOn); 
214         if (isOr)  transform->SetActive( transform->IsActive() || setOn); 
215       }
216     }
217   }
218 }
219
220
221 void AliTPCkalmanFit::Update(const AliTPCkalmanFit * kalman){
222   //
223   // Update Kalman filter
224   //
225   Int_t ncalibs = fCalibration->GetEntries();
226   TMatrixD vecXk=*fCalibParam;       // X vector
227   TMatrixD covXk=*fCalibCovar;       // X covariance
228   TMatrixD &vecZk = *(kalman->fCalibParam);
229   TMatrixD &measR = *(kalman->fCalibCovar);
230
231   TMatrixD matHk(ncalibs,ncalibs);   // vector to mesurement
232   TMatrixD vecYk(ncalibs,1);         // Innovation or measurement residual
233   TMatrixD matHkT(ncalibs,ncalibs);  // helper matrix Hk transpose
234   TMatrixD matSk(ncalibs,ncalibs);   // Innovation (or residual) covariance
235   TMatrixD matKk(ncalibs,ncalibs);   // Optimal Kalman gain
236   TMatrixD covXk2(ncalibs,ncalibs);  // helper matrix
237   TMatrixD covXk3(ncalibs,ncalibs);  // helper matrix
238   //
239   for (Int_t i=0;i<ncalibs;i++){
240     for (Int_t j=0;j<ncalibs;j++) matHk(i,j)=0;
241     matHk(i,i)=1;
242   }
243    vecYk = vecZk-matHk*vecXk;               // Innovation or measurement residual
244    matHkT=matHk.T(); matHk.T();
245    matSk = (matHk*(covXk*matHkT))+measR;    // Innovation (or residual) covariance
246    matSk.Invert();
247    matKk = (covXk*matHkT)*matSk;            //  Optimal Kalman gain
248    vecXk += matKk*vecYk;                    //  updated vector
249    covXk2= (matHk-(matKk*matHk));
250    covXk3 =  covXk2*covXk;
251    covXk = covXk3;
252    (*fCalibParam) = vecXk;
253    (*fCalibCovar) = covXk;
254 }
255
256
257
258
259
260 void AliTPCkalmanFit::FitTrackLinear(AliTrackPointArray &points, TTreeSRedirector *debug){
261   //
262   //
263
264   if (!fCalibParam) {
265     AliError("Kalman Fit not initialized");
266     return;
267   }
268   //
269   // 1. Make initial track hypothesis
270   //
271   TLinearFitter lfitY(2,"pol1");
272   TLinearFitter lfitZ(2,"pol1");
273   TVectorD vecZ(2);
274   TVectorD vecY(2);
275   //
276   lfitY.StoreData(kTRUE);
277   lfitZ.StoreData(kTRUE);
278   lfitY.ClearPoints();
279   lfitZ.ClearPoints();  
280   Int_t npoints = points.GetNPoints();
281   if (npoints<2) return;
282   const Double_t kFac=npoints*npoints*100;
283   const Double_t kOff0=0.01*0.01;
284   const Double_t kOff1=kOff0/(250.*250.);
285   //
286   // 0. Make seed
287   //
288   //  AliTrackPointArray pointsc(points);
289   //ApplyCalibration(&pointsc, -1.);
290   //
291   // 1.a Choosing reference rotation alpha
292   //
293   fCurrentAlpha = TMath::ATan2(points.GetY()[npoints-1]-points.GetY()[0], points.GetX()[npoints-1]-points.GetX()[0]);  
294   fCA = TMath::Cos(fCurrentAlpha);
295   fSA = TMath::Sin(fCurrentAlpha);
296   //
297   // 1.b Fit the track in the rotated frame - MakeSeed 
298   //
299   for (Int_t ipoint=0; ipoint<npoints-1; ipoint++){
300     Double_t rx =   fCA*points.GetX()[ipoint]+fSA*points.GetY()[ipoint];
301     Double_t ry =  -fSA*points.GetX()[ipoint]+fCA*points.GetY()[ipoint];
302     Double_t rz =  points.GetZ()[ipoint];
303     lfitY.AddPoint(&rx,ry,1);
304     lfitZ.AddPoint(&rx,rz,1);
305   }
306   lfitY.Eval();
307   lfitZ.Eval();
308   lfitY.GetParameters(vecY);
309   lfitZ.GetParameters(vecZ);
310   //
311   // 2. Set initial parameters and the covariance matrix
312   //
313   Int_t ncalibs = fCalibration->GetEntries();
314   if (!fLinearParam) {
315     fLinearParam = new TMatrixD(ncalibs+4,1);
316     fLinearCovar = new TMatrixD(ncalibs+4,ncalibs+4);    
317   }
318   //
319   for (Int_t i=0; i<ncalibs+4;i++){
320     (*fLinearParam)(i,0)=0;
321     if (i<ncalibs) (*fLinearParam)(i,0) = (*fCalibParam)(i,0);
322     for (Int_t j=0; j<ncalibs+4;j++){
323       (*fLinearCovar)(i,j)=0;
324       if (i<ncalibs&&j<ncalibs) (*fLinearCovar)(i,j) = (*fCalibCovar)(i,j);
325     }
326   }
327   Double_t chi2Y = lfitY.GetChisquare()/lfitY.GetNpoints();
328   Double_t chi2Z = lfitZ.GetChisquare()/lfitZ.GetNpoints();
329   (*fLinearParam)(ncalibs+0,0) =  lfitY.GetParameter(0);
330   (*fLinearCovar)(ncalibs+0,ncalibs+0)= lfitY.GetCovarianceMatrixElement(0,0)*chi2Y*kFac+kOff0;
331   (*fLinearParam)(ncalibs+1,0) =  lfitY.GetParameter(1);
332   (*fLinearCovar)(ncalibs+1,ncalibs+1)= lfitY.GetCovarianceMatrixElement(1,1)*chi2Y*kFac+kOff1;
333   //
334   (*fLinearParam)(ncalibs+2,0) =  lfitZ.GetParameter(0);
335   (*fLinearCovar)(ncalibs+2,ncalibs+2)= lfitZ.GetCovarianceMatrixElement(0,0)*chi2Z*kFac+kOff0;
336   (*fLinearParam)(ncalibs+3,0) =  lfitZ.GetParameter(1);
337   (*fLinearCovar)(ncalibs+3,ncalibs+3)= lfitZ.GetCovarianceMatrixElement(1,1)*chi2Z*kFac+kOff1;
338   //
339   // Fit thetrack together with correction
340   //
341   AliTrackPoint point;
342   for (Int_t ipoint=0; ipoint<npoints-1; ipoint++){
343     //
344     if (!points.GetPoint(point,ipoint)) continue;
345     Double_t erry2 = chi2Y;
346     Double_t errz2 = chi2Z;
347     //set error - it is hack
348     Float_t *cov = (Float_t*) point.GetCov();
349     cov[1] = erry2+kOff0;
350     cov[2] = errz2+kOff0;
351     UpdateLinear(point,debug);
352     if (!points.GetPoint(point,npoints-1-ipoint)) continue;
353     //set error - it is hack
354     cov = (Float_t*) point.GetCov();
355     cov[1] = erry2+kOff0;
356     cov[2] = errz2+kOff0;
357     UpdateLinear(point,debug);
358   }
359   //
360   // save current param and covariance
361   for (Int_t i=0; i<ncalibs;i++){
362     AliTPCTransformation * transform = (AliTPCTransformation *)fCalibration->At(i);
363     transform->fParam= (*fLinearParam)(i,0);
364     (*fCalibParam)(i,0) = (*fLinearParam)(i,0);
365     for (Int_t j=0; j<ncalibs;j++){
366       (*fCalibCovar)(i,j) = (*fLinearCovar)(i,j);
367     }
368   }
369   if (debug){ // dump debug info
370     (*debug)<<"fitLinear"<<
371       "vecY.="<<&vecY<<
372       "vecZ.="<<&vecZ<<
373       "chi2Y="<<chi2Y<<
374       "chi2Z="<<chi2Z<<
375       "lP.="<<fLinearParam<<
376       "cP.="<<fCalibParam<<
377       "lC.="<<fLinearCovar<<
378       "cC.="<<fCalibCovar<<
379       "\n";
380   }
381 }
382
383 void AliTPCkalmanFit::DumpTrackLinear(AliTrackPointArray &points, TTreeSRedirector *debug){
384   //
385   // Dump the track parameters before and after current calibration
386   //
387   // Track is divided to two parts - 
388   // X mean is defined as middle point 
389   //
390
391   if (!fCalibParam) {
392     AliError("Kalman Fit not initialized");
393     return;
394   }
395   //
396   // 
397   //
398   TLinearFitter *fitters[16];
399   TVectorD      *params[16];
400   TVectorD      *errs[16];
401   TVectorD      chi2N(16);
402   Int_t npoints = points.GetNPoints();
403   AliTrackPointArray pointsTrans(points);
404   ApplyCalibration(&pointsTrans,-1.);
405   for (Int_t ifit=0; ifit<8;ifit++){
406     fitters[ifit]  = new TLinearFitter(2,"pol1");
407     params[ifit]   = new TVectorD(2);
408     fitters[ifit+8]= new TLinearFitter(3,"pol2");
409     params[ifit+8] = new TVectorD(3);
410     errs[ifit]     = new TVectorD(2);
411     errs[ifit+8]   = new TVectorD(3);
412   }
413   //
414   // calculate mean x point  and corrdinate frame
415   //
416   fCurrentAlpha = TMath::ATan2(points.GetY()[npoints-1]-points.GetY()[0], points.GetX()[npoints-1]-points.GetX()[0]);  
417   fCA = TMath::Cos(fCurrentAlpha);
418   fSA = TMath::Sin(fCurrentAlpha);
419   Double_t xmean=0, sum=0;
420   for (Int_t ipoint=0; ipoint<npoints-1; ipoint++){
421     Double_t rx  =   fCA*points.GetX()[ipoint]+fSA*points.GetY()[ipoint];
422     xmean+=rx;
423     sum++;
424   }
425   xmean/=sum;
426   //
427   for (Int_t ipoint=0; ipoint<npoints-1; ipoint++){
428     Double_t rx  =   fCA*points.GetX()[ipoint]+fSA*points.GetY()[ipoint];
429     Double_t ry  =  -fSA*points.GetX()[ipoint]+fCA*points.GetY()[ipoint];
430     Double_t rz  =  points.GetZ()[ipoint];
431     //
432     Double_t rxT =   fCA*pointsTrans.GetX()[ipoint]+fSA*pointsTrans.GetY()[ipoint];
433     Double_t ryT =  -fSA*pointsTrans.GetX()[ipoint]+fCA*pointsTrans.GetY()[ipoint];
434     Double_t rzT =  pointsTrans.GetZ()[ipoint];
435     if (rx>xmean){
436       fitters[0]->AddPoint(&rxT, ryT,1);
437       fitters[2]->AddPoint(&rxT, rzT,1);
438       fitters[4]->AddPoint(&rx, ry,1);
439       fitters[6]->AddPoint(&rx, rz,1);
440       fitters[8]->AddPoint(&rxT, ryT,1);
441       fitters[10]->AddPoint(&rxT, rzT,1);
442       fitters[12]->AddPoint(&rx, ry,1);
443       fitters[14]->AddPoint(&rx, rz,1);
444     }else{
445       fitters[1]->AddPoint(&rxT, ryT,1);
446       fitters[3]->AddPoint(&rxT, rzT,1);
447       fitters[5]->AddPoint(&rx, ry,1);
448       fitters[7]->AddPoint(&rx, rz,1);
449       fitters[9]->AddPoint(&rxT, ryT,1);
450       fitters[11]->AddPoint(&rxT, rzT,1);
451       fitters[13]->AddPoint(&rx, ry,1);
452       fitters[15]->AddPoint(&rx, rz,1);
453     }
454   }
455   for (Int_t ifit=0;ifit<16;ifit++){
456     fitters[ifit]->Eval();
457     fitters[ifit]->GetParameters(*(params[ifit]));
458     fitters[ifit]->GetErrors(*(errs[ifit]));
459     chi2N[ifit] = TMath::Sqrt(fitters[ifit]->GetChisquare()/(fitters[ifit]->GetNpoints()-1));
460     (*(errs[ifit]))[0]*=chi2N[ifit];
461     (*(errs[ifit]))[1]*=chi2N[ifit];
462     if (ifit>=8) (*(errs[ifit]))[2]*=chi2N[ifit];  // second derivative
463   }
464   if (debug){
465     (*debug)<<"dumpLinear"<<
466       "alpha="<<fCurrentAlpha<<
467       "xmean="<<xmean<<
468       "y0T.="<<params[0]<<
469       "y1T.="<<params[1]<<
470       "z0T.="<<params[2]<<
471       "z1T.="<<params[3]<<
472       "y0O.="<<params[4]<<
473       "y1O.="<<params[5]<<
474       "z0O.="<<params[6]<<
475       "z1O.="<<params[7]<<
476       "y0T2.="<<params[8]<<
477       "y1T2.="<<params[9]<<
478       "z0T2.="<<params[10]<<
479       "z1T2.="<<params[11]<<
480       "y0O2.="<<params[12]<<
481       "y1O2.="<<params[13]<<
482       "z0O2.="<<params[14]<<
483       "z1O2.="<<params[15]<<
484       "y0TErr.="<<errs[0]<<
485       "y1TErr.="<<errs[1]<<
486       "z0TErr.="<<errs[2]<<
487       "z1TErr.="<<errs[3]<<
488       "y0OErr.="<<errs[4]<<
489       "y1OErr.="<<errs[5]<<
490       "z0OErr.="<<errs[6]<<
491       "z1OErr.="<<errs[7]<<
492       "y0T2Err.="<<errs[8]<<
493       "y1T2Err.="<<errs[9]<<
494       "z0T2Err.="<<errs[10]<<
495       "z1T2Err.="<<errs[11]<<
496       "y0O2Err.="<<errs[12]<<
497       "y1O2Err.="<<errs[13]<<
498       "z0O2Err.="<<errs[14]<<
499       "z1O2Err.="<<errs[15]<<
500       "chi2.="<<&chi2N<<
501       "\n";
502   }
503   //
504   //                    delta  alpha   y0     z0    ky   kz 
505   Double_t x[6]={0,0,0,0,0,0};
506   x[1]=fCurrentAlpha;
507   x[2]=(*params[0])[0];
508   x[3]=(*params[2])[0];
509   x[4]=(*params[0])[1];
510   x[5]=(*params[2])[1];
511   //
512   //Delta y
513   //
514   x[0]= (*params[1])[0]-(*params[0])[0];
515   fLinearTrackDelta[0]->Fill(x);
516   x[0]/=TMath::Sqrt((*errs[1])[0]*(*errs[1])[0]+(*errs[0])[0]*(*errs[0])[0]);
517   fLinearTrackPull[0]->Fill(x);
518   //
519   //Delta ky
520   //
521   x[0]= 100*((*params[1])[1]-(*params[0])[1]);
522   fLinearTrackDelta[1]->Fill(x);
523   x[0]/=100*TMath::Sqrt((*errs[1])[1]*(*errs[1])[1]+(*errs[0])[1]*(*errs[0])[1]);
524   fLinearTrackPull[1]->Fill(x);
525   //
526   // ky2_0
527   //
528   x[0]= 100*100*((*params[8])[2]);
529   fLinearTrackDelta[2]->Fill(x);
530   x[0]/=100*100*TMath::Sqrt((*errs[8])[2]*(*errs[8])[2]);
531   fLinearTrackPull[2]->Fill(x);
532   //
533   // ky2_1
534   //
535   x[0]= 100*100*((*params[9])[2]);
536   fLinearTrackDelta[3]->Fill(x);
537   x[0]/=100*100*TMath::Sqrt((*errs[9])[2]*(*errs[9])[2]);
538   fLinearTrackPull[3]->Fill(x);
539   //
540   //
541   //Delta z
542   //
543   x[0]= (*params[3])[0]-(*params[2])[0];
544   fLinearTrackDelta[4]->Fill(x);
545   x[0]/=TMath::Sqrt((*errs[3])[0]*(*errs[3])[0]+(*errs[2])[0]*(*errs[2])[0]);
546   fLinearTrackPull[4]->Fill(x);
547   //
548   //Delta kz
549   //
550   x[0]= 100*((*params[3])[1]-(*params[2])[1]);
551   fLinearTrackDelta[5]->Fill(x);
552   x[0]/=100*TMath::Sqrt((*errs[3])[1]*(*errs[3])[1]+(*errs[2])[1]*(*errs[2])[1]);
553   fLinearTrackPull[5]->Fill(x);
554   //
555   // kz2_0
556   //
557   x[0]= 100*100*((*params[10])[2]);
558   fLinearTrackDelta[6]->Fill(x);
559   x[0]/=100*100*TMath::Sqrt((*errs[10])[2]*(*errs[10])[2]);
560   fLinearTrackPull[6]->Fill(x);
561   //
562   // kz2_1
563   //
564   x[0]= 100*100*((*params[11])[2]);
565   fLinearTrackDelta[7]->Fill(x);
566   x[0]/=100*100*TMath::Sqrt((*errs[11])[2]*(*errs[11])[2]);
567   fLinearTrackPull[7]->Fill(x);
568
569   //
570   // rms of track
571   //
572   x[0]= chi2N[0];
573   fLinearTrackDelta[8]->Fill(x);
574   x[0]= chi2N[1];
575   fLinearTrackDelta[9]->Fill(x);
576   x[0]= chi2N[2];
577   fLinearTrackDelta[10]->Fill(x);
578   x[0]= chi2N[3];
579   fLinearTrackDelta[11]->Fill(x);
580   //
581
582
583   for (Int_t ifit=0; ifit<8;ifit++){
584     delete fitters[ifit];
585     delete params[ifit];
586     delete fitters[ifit+8];
587     delete params[ifit+8];
588     delete errs[ifit];
589     delete errs[ifit+8];
590   }
591 }
592
593
594 void AliTPCkalmanFit::Propagate(TTreeSRedirector */*debug*/){
595   //
596   // Propagate the Kalman
597   //
598 }
599
600
601 void AliTPCkalmanFit::PropagateTime(Int_t time){
602   //
603   // Propagate the calibration in time
604   // - Increase covariance matrix
605   //
606   if (fCalibCovar) return;
607   Int_t ncalibs = fCalibration->GetEntries();
608   Double_t deltaT = 0;
609   if (fLastTimeStamp>0) deltaT = (fLastTimeStamp-time)/(60*60.);  // delta T in hours
610   fLastTimeStamp = time;  
611   for (Int_t icalib=0;icalib<ncalibs; icalib++){
612     AliTPCTransformation * transform = (AliTPCTransformation *)fCalibration->At(icalib);
613     (*fCalibCovar)(icalib,icalib)+=  transform->fSigma2Time*deltaT;
614   }
615 }
616
617
618 void  AliTPCkalmanFit::UpdateLinear(AliTrackPoint &point, TTreeSRedirector *debug){
619   //
620   // Update Kalman
621   //
622   //
623   Int_t ncalibs = fCalibration->GetEntries();
624   Int_t kNmeas  = 2; 
625   Int_t nelem   = ncalibs+4;
626   TMatrixD &vecXk=*fLinearParam;     // X vector
627   TMatrixD &covXk=*fLinearCovar;     // X covariance
628   //
629   TMatrixD matHk(kNmeas,nelem);     // vector to mesurement
630   TMatrixD vecYk(kNmeas,1);         // Innovation or measurement residual
631   TMatrixD vecZk(kNmeas,1);         // Innovation or measurement residual
632   TMatrixD measR(kNmeas,kNmeas);
633   TMatrixD matHkT(nelem,kNmeas);    // helper matrix Hk transpose
634   TMatrixD matSk(kNmeas,kNmeas);    // Innovation (or residual) covariance
635   TMatrixD matKk(nelem,kNmeas);     // Optimal Kalman gain
636   TMatrixD covXk2(nelem,nelem);     // helper matrix
637   TMatrixD covXk3(nelem,nelem);     // helper matrix
638   TMatrixD mat1(nelem,nelem);
639   //
640   // reset matHk
641   for (Int_t iel=0;iel<nelem;iel++){
642     for (Int_t ip=0;ip<kNmeas;ip++) {
643       matHk(ip,iel)=0; 
644     }
645   }
646   for (Int_t iel0=0;iel0<nelem;iel0++)
647     for (Int_t iel1=0;iel1<nelem;iel1++){
648       if (iel0!=iel1) mat1(iel0,iel1)=0;
649       if (iel0==iel1) mat1(iel0,iel1)=1;
650     }
651   //
652   // fill matrix Hk
653   //
654   Int_t volId = point.GetVolumeID();
655   Double_t gxyz[3]={point.GetX(), point.GetY(),point.GetZ()};
656   Double_t rxyz[3]={fCA*gxyz[0]+fSA*gxyz[1],-fSA*gxyz[0]+fCA*gxyz[1] ,point.GetZ()};
657   //
658   Double_t dxdydz[3]={0,0,0};
659   Double_t rdxdydz[3]={0,0,0};
660   
661   for (Int_t icalib=0;icalib<ncalibs;icalib++){
662     AliTPCTransformation * transform = (AliTPCTransformation *)fCalibration->At(icalib);
663     dxdydz[0] = transform->GetDeltaXYZ(0,volId, 1, gxyz[0], gxyz[1], gxyz[2]); 
664     dxdydz[1] = transform->GetDeltaXYZ(1,volId, 1, gxyz[0], gxyz[1], gxyz[2]);  
665     dxdydz[2] = transform->GetDeltaXYZ(2,volId, 1, gxyz[0], gxyz[1], gxyz[2]);
666     rdxdydz[0] =  fCA*dxdydz[0]+fSA*dxdydz[1]; 
667     rdxdydz[1] = -fSA*dxdydz[0]+fCA*dxdydz[1]; 
668     rdxdydz[2] =  dxdydz[2]; 
669     //
670     matHk(0,icalib)= rdxdydz[1]-rdxdydz[0]*(*fLinearParam)(ncalibs+1,0);  // shift y + shift x * angleY
671     matHk(1,icalib)= rdxdydz[2]-rdxdydz[0]*(*fLinearParam)(ncalibs+3,0);  // shift z + shift x * angleZ
672   }
673   matHk(0,ncalibs+0)=1;
674   matHk(0,ncalibs+1)=rxyz[0];
675   matHk(1,ncalibs+2)=1;
676   matHk(1,ncalibs+3)=rxyz[0];
677   //
678   //
679   //
680   vecZk(0,0) =  rxyz[1];
681   vecZk(1,0) =  rxyz[2];
682   measR(0,0) = point.GetCov()[1]; measR(0,1)=0;
683   measR(1,1) = point.GetCov()[2]; measR(1,0)=0;
684   //
685   vecYk = vecZk-matHk*vecXk;               // Innovation or measurement residual
686   matHkT=matHk.T(); matHk.T();
687   matSk = (matHk*(covXk*matHkT))+measR;    // Innovation (or residual) covariance
688   matSk.Invert();
689   matKk = (covXk*matHkT)*matSk;            //  Optimal Kalman gain
690   //
691   covXk2= (mat1-(matKk*matHk));
692   covXk3 =  covXk2*covXk;          
693   //
694   CheckCovariance(covXk3,100);
695   vecXk += matKk*vecYk;                    //  updated vector 
696   covXk = covXk3;  
697   if (debug){ // dump debug info
698     (*debug)<<"updateLinear"<<
699       //
700       "point.="<<&point<<
701       "vecYk.="<<&vecYk<<
702       "vecZk.="<<&vecZk<<
703       "measR.="<<&measR<<
704       "matHk.="<<&matHk<<
705       "matHkT.="<<&matHkT<<
706       "matSk.="<<&matSk<<
707       "matKk.="<<&matKk<<
708       "covXk2.="<<&covXk2<<
709       "covXk.="<<&covXk<<
710       "vecXk.="<<&vecXk<<
711       "\n";
712   }  
713 }
714
715
716 AliTrackPointArray * AliTPCkalmanFit::SortPoints(AliTrackPointArray &points){
717   //
718   //Creates the array  - points sorted according radius - neccessay for kalman fit
719   // 
720   //
721   // 0. choose the frame - rotation angle
722   //
723   Int_t npoints = points.GetNPoints();
724   if (npoints<1) return 0;
725   Double_t currentAlpha = TMath::ATan2(points.GetY()[npoints-1]-points.GetY()[0], points.GetX()[npoints-1]-points.GetX()[0]);  
726   Double_t ca = TMath::Cos(currentAlpha);
727   Double_t sa = TMath::Sin(currentAlpha);
728   //
729   // 1. sort the points
730   //
731   Double_t *rxvector = new Double_t[npoints];
732   Int_t    *indexes  = new Int_t[npoints];
733   for (Int_t ipoint=0; ipoint<npoints-1; ipoint++){
734     rxvector[ipoint]=ca*points.GetX()[ipoint]+sa*points.GetY()[ipoint];
735   }
736   TMath::Sort(npoints, rxvector,indexes,kFALSE);
737   AliTrackPoint point;
738   AliTrackPointArray *pointsSorted= new AliTrackPointArray(npoints);
739   for (Int_t ipoint=0; ipoint<npoints; ipoint++){
740     if (!points.GetPoint(point,indexes[ipoint])) continue;
741     pointsSorted->AddPoint(ipoint,&point);
742   }
743   delete [] rxvector;
744   delete [] indexes;
745   return pointsSorted;
746 }
747
748
749
750 AliTrackPointArray * AliTPCkalmanFit::MakePointArrayLinear(Double_t alpha, Double_t y0, Double_t z0, Double_t ky, Double_t kz, Double_t err){
751   //
752   //
753   //
754   AliTrackPointArray array(500);
755   Float_t cov[10];  // dummy covariance
756   Int_t npoints=0;
757   for (Int_t i=0;i<6;i++) cov[i]=0.001;
758   for (Int_t i=0;i<500;i++){    
759     AliTrackPoint point(0, 0, 0, cov, -1,0,0);
760     array.AddPoint(npoints, &point);
761     npoints++;
762   }
763   npoints=0;
764   for (Float_t ir = -245; ir<245; ir++){
765     //
766     //
767     if (TMath::Abs(ir)<80) continue;
768     Double_t ca = TMath::Cos(alpha);
769     Double_t sa = TMath::Sin(alpha);
770     Double_t lx = ir;
771     Double_t ly = y0+lx*ky+gRandom->Gaus(0,err);
772     Double_t lz = z0+lx*kz+gRandom->Gaus(0,err);
773     Double_t gx = lx*ca-ly*sa;
774     Double_t gy = lx*sa+ly*ca;
775     Double_t gz = lz;
776     Double_t galpha = TMath::ATan2(gy,gx);
777     Int_t isec = TMath::Nint((9*galpha/TMath::Pi()+0.5));
778     if (isec<0) isec+=18;
779     if (gz<0) isec+=18;
780     if (ir>130) isec+=36;
781     //
782     AliTrackPoint point(gx, gy, gz, cov, isec,0,0);
783     array.AddPoint(npoints, &point);
784     npoints++;
785   }
786   AliTrackPointArray *parray = new AliTrackPointArray(npoints);
787   AliTrackPoint point;
788   for (Int_t i=0;i<npoints;i++){
789     array.GetPoint(point,i);
790     parray->AddPoint(i,&point);
791   }
792
793   return parray;
794 }
795
796 void AliTPCkalmanFit::AddCalibration(AliTPCTransformation * calib){
797   //
798   // Add calibration
799   //
800   if (!fCalibration) fCalibration = new TObjArray(2000);
801   fCalibration->AddLast(calib);
802 }
803
804 void AliTPCkalmanFit::ApplyCalibration(AliTrackPointArray *array, Double_t csign){
805   //
806   //
807   //
808   if (!fCalibration) return;
809   Int_t ncalibs = fCalibration->GetEntries();
810   if (ncalibs==0) return;
811   Int_t npoints = array->GetNPoints();
812   for (Int_t ipoint=0; ipoint<npoints; ipoint++){
813     Int_t volId = array->GetVolumeID()[ipoint];
814     Double_t xyz[3]={array->GetX()[ipoint], array->GetY()[ipoint],array->GetZ()[ipoint]};
815     Double_t dxdydz[3]={0,0,0};
816     for (Int_t icalib=0; icalib<ncalibs; icalib++){
817       AliTPCTransformation * transform = (AliTPCTransformation *)fCalibration->At(icalib);
818       dxdydz[0] += transform->GetDeltaXYZ(0,volId, transform->fParam, xyz[0], xyz[1], xyz[2]); 
819       dxdydz[1] += transform->GetDeltaXYZ(1,volId, transform->fParam, xyz[0], xyz[1], xyz[2]);  
820       dxdydz[2] += transform->GetDeltaXYZ(2,volId, transform->fParam, xyz[0], xyz[1], xyz[2]);  
821     }
822     ((Float_t*)array->GetX())[ipoint]+=csign*dxdydz[0];
823     ((Float_t*)array->GetY())[ipoint]+=csign*dxdydz[1];
824     ((Float_t*)array->GetZ())[ipoint]+=csign*dxdydz[2];
825   }
826 }
827
828 Bool_t AliTPCkalmanFit::DumpCorelation(Double_t threshold,  const char *mask0, const char *mask1){
829   //
830   //
831   //
832   TMatrixD &mat = *fCalibCovar;
833   Int_t nrow= mat.GetNrows();
834   for (Int_t irow=0; irow<nrow; irow++){
835     AliTPCTransformation * trans0 = GetTransformation(irow);
836     TString  strName0(trans0->GetName());
837     if (mask0){
838       if (!strName0.Contains(mask0)) continue;
839     }
840     for (Int_t icol=irow+1; icol<nrow; icol++){
841       AliTPCTransformation * trans1 = GetTransformation(icol);
842       TString  strName1(trans1->GetName());
843       if (mask1){
844         if (!strName1.Contains(mask1)) continue;
845       } 
846       //
847       Double_t diag = TMath::Sqrt(TMath::Abs(mat(irow,irow)*mat(icol,icol))); 
848       if (diag<=0){
849         printf("Negative covariance\t%d\t%d\t%f\n",irow,icol, mat(irow,icol));
850         continue;
851       }
852       Double_t corr0 = mat(irow,icol)/diag;
853       if (TMath::Abs(corr0)>threshold){
854         printf("%d\t%d\t%s\t%s\t%f\t%f\t%f\n", irow,icol, trans0->GetName(), trans1->GetName(),
855                TMath::Sqrt(mat(irow,irow)), TMath::Sqrt(mat(icol,icol)), corr0);
856       }
857     }
858   }
859   return (nrow>0);
860 }
861
862 Bool_t AliTPCkalmanFit::DumpCalib(const char *mask){
863   //
864   // Print calibration entries - name, value, error
865   //
866   TMatrixD &mat = *fCalibCovar;
867   Int_t nrow= mat.GetNrows();
868   TString  strMask(mask);
869   for (Int_t irow=0; irow<nrow; irow++){
870     AliTPCTransformation * trans0 = GetTransformation(irow);
871     TString  strName(trans0->GetName());
872     if (mask){
873       if (!strName.Contains(mask)) continue;
874     }
875     printf("%d\t%s\t%f\t%f\n", 
876            irow, 
877            trans0->GetName(),
878            (*fCalibParam)(irow,0),
879            TMath::Sqrt(mat(irow,irow)));          
880   }
881   return (nrow>0);
882 }
883
884
885 Bool_t  AliTPCkalmanFit::CheckCovariance(TMatrixD &mat, Float_t /*maxEl*/){
886   //
887   // Check consistency of covariance matrix
888   // + symetrize coavariance matrix
889   Bool_t isOK=kTRUE;
890   Int_t nrow= mat.GetNrows();
891   for (Int_t irow=0; irow<nrow; irow++){
892     if (mat(irow,irow)<=0){
893       printf("Negative covariance\t%d\t%f\n",irow,mat(irow,irow));
894       isOK=kFALSE;
895     }
896 //     if (mat(irow,irow)>maxEl){
897 //       printf("Too big  covariance\t%d\t%f\n",irow,mat(irow,irow));
898 //       isOK=kFALSE;
899 //     }
900     
901     for (Int_t icol=0; icol<nrow; icol++){
902       //      if (mat(irow,irow)
903       Double_t diag = TMath::Sqrt(TMath::Abs(mat(irow,irow)*mat(icol,icol))); 
904       if (diag<=0){
905         printf("Negative covariance\t%d\t%d\t%f\n",irow,icol, mat(irow,icol));
906         isOK=kFALSE;
907         continue;
908       }
909       Double_t cov0 = mat(irow,icol)/diag;
910       Double_t cov1 = mat(icol,irow)/diag;
911       if (TMath::Abs(cov0)>1 || TMath::Abs(cov1)>1 ){
912         printf("Covariance Problem %d\t%d\t%f\t%f\n",irow,icol, cov0, cov1);
913         isOK=kFALSE;
914       }
915       if (TMath::Abs(cov0-cov1)>0.0000001){
916         printf("Asymetry problem %d\t%d\t%f\t%f\n",irow,icol, cov0, cov1);
917         isOK=kFALSE;
918       }
919       //
920       // symetrize the covariance matrix
921       Double_t mean = (mat(irow,icol)+ mat(icol,irow))*0.5;
922       mat(irow,icol)=mean;
923       mat(icol,irow)=mean;
924     }
925   }
926   return isOK;
927 }
928
929
930 AliTPCkalmanFit *  AliTPCkalmanFit::Test(Int_t ntracks){
931   //
932   // This is test example
933   //
934
935   //
936   // 1. Setup transformation
937   //
938
939
940   TVectorD fpar(10);
941   AliTPCTransformation * transformation=0;
942   AliTPCkalmanFit * kalmanFit0 =  new AliTPCkalmanFit;
943   AliTPCkalmanFit * kalmanFit2 =  new AliTPCkalmanFit;
944   //
945   // Radial scaling
946   //
947   for (Int_t iside=0; iside<=1; iside++)
948     for (Int_t ipar0=0; ipar0<3; ipar0++)
949       for (Int_t ipar1=0; ipar1<3; ipar1++){
950         fpar[0]=ipar0; 
951         fpar[1]=ipar1;
952         if (ipar0+ipar1==0) continue;
953         Double_t param = (gRandom->Rndm()-0.5)*0.5;  // generate random parameters
954         char tname[100];
955         sprintf(tname,"tscalingR%d%dSide%d",ipar0,ipar1,iside);
956         transformation = new AliTPCTransformation(tname,AliTPCTransformation::BitsSide(iside),"TPCscalingRPol",0,0,1);
957         transformation->SetParams(0,5*0.25,0,&fpar);
958         kalmanFit0->AddCalibration(transformation);
959         transformation = new AliTPCTransformation(tname,AliTPCTransformation::BitsSide(iside),"TPCscalingRPol",0,0,1);
960         transformation->SetParams(param,0.25,0,&fpar);
961         kalmanFit2->AddCalibration(transformation);
962       }
963
964
965   //
966   // 2. Init transformation
967   //
968   kalmanFit2->Init();    
969   kalmanFit0->Init();
970   
971   //
972   // 3. Run kalman filter
973   //
974   Int_t ncalibs = kalmanFit0->fCalibration->GetEntries();
975   TVectorD err(ncalibs);
976   TTreeSRedirector *pcstream = new TTreeSRedirector("kalmanfitTest.root");
977   for (Int_t i=0;i<ntracks;i++){
978     if (i%100==0) printf("%d\n",i);
979      Double_t alpha = gRandom->Rndm()*TMath::TwoPi();
980      Double_t   y0  = (gRandom->Rndm()-0.5)*180; 
981      Double_t   z0  = (gRandom->Rndm()-0.5)*250*2; 
982      Double_t   ky  = (gRandom->Rndm()-0.5)*1; 
983      Double_t   kz  = (gRandom->Rndm()-0.5)*1;
984      //generate random TPC track
985      AliTrackPointArray * array  = AliTPCkalmanFit::MakePointArrayLinear(alpha,y0,z0, ky, kz,0.04);
986      AliTrackPointArray * arrayB = new AliTrackPointArray(*array);  // backup
987      kalmanFit2->ApplyCalibration(array,1.);  // misalign ideal track
988      for (Int_t icalib=0; icalib<ncalibs; icalib++){
989        err[icalib] = TMath::Sqrt((*kalmanFit0->fCalibCovar)(icalib,icalib));
990      }
991      //
992      (*pcstream)<<"dump0"<<
993        "alpha="<<alpha<<
994        "y0="<<y0<<
995        "z0="<<z0<<
996        "ky="<<ky<<
997        "lz="<<kz<<
998        "p.="<<array<<
999        "pB.="<<arrayB<<
1000        "cparam.="<<kalmanFit0->fCalibParam<<
1001        "ccovar.="<<kalmanFit0->fCalibCovar<<
1002        "err.="<<&err<<
1003        "gparam.="<<kalmanFit2->fCalibParam<<
1004        "gcovar.="<<kalmanFit2->fCalibCovar<<
1005        "\n";
1006      if (i%20==0) {
1007        kalmanFit0->FitTrackLinear(*array,pcstream); // fit track - dump intermediate results
1008      }else{
1009        kalmanFit0->FitTrackLinear(*array,0);        // fit track + calibration
1010      }
1011      kalmanFit0->DumpTrackLinear(*array,pcstream);    // dump track residuals to the tree + fill histograms
1012   }
1013   pcstream->GetFile()->cd();
1014   kalmanFit0->Write("kalmanFit");
1015   delete pcstream;
1016   return kalmanFit0;
1017 }
1018
1019
1020 Double_t AliTPCkalmanFit::GetTPCDeltaXYZ(Int_t coord, Int_t volID, Double_t x, Double_t y, Double_t z){
1021   //
1022   // function for visualization purposes
1023   //
1024   if (!fCalibration) return 0;
1025   Int_t ncalibs = fCalibration->GetEntries();
1026   if (ncalibs==0) return 0;
1027   Double_t dxdydz[3]={0,0,0};
1028   //
1029   if (volID<0){
1030     Double_t alpha       = TMath::ATan2(y,x);
1031     Double_t r           = TMath::Sqrt(y*y+x*x);
1032     volID                = TMath::Nint(9*alpha/TMath::Pi()-0.5);
1033     if (volID<0) volID+=18;
1034     if (z<0) volID+=18;
1035     if (r>120) volID+=36;
1036   }
1037   for (Int_t icalib=0; icalib<ncalibs; icalib++){
1038     AliTPCTransformation * transform = (AliTPCTransformation *)fCalibration->At(icalib);
1039     Double_t param = (*fCalibParam)(icalib,0);
1040     dxdydz[coord] += transform->GetDeltaXYZ(coord,volID, param, x, y,z); 
1041   }
1042   return dxdydz[coord];
1043 }
1044
1045 Double_t AliTPCkalmanFit::SGetTPCDeltaXYZ(Int_t coord, Int_t volID, Double_t x, Double_t y, Double_t z){
1046   //
1047   //
1048   //
1049   if (AliTPCkalmanFit::fgInstance==0) return 0;
1050   return AliTPCkalmanFit::fgInstance->GetTPCDeltaXYZ(coord, volID,x,y,z);
1051 }
1052