L3 becomes HLT
[u/mrichter/AliRoot.git] / HLT / kalman / AliHLTKalmanTrack.cxx
1 #include "TMath.h"
2 #include "AliHLTKalmanTrack.h"
3 #include "AliHLTSpacePointData.h"
4 #include "AliHLTLogging.h"
5 #include "AliHLTTransform.h"
6 #include "AliHLTStandardIncludes.h"
7
8 // includes for offline comparison, will be removed
9 #include "AliTPCtrack.h"
10 // includes for offline comparison, will be removed
11
12 #include "Riostream.h" 
13
14
15 ClassImp(AliHLTKalmanTrack)
16
17 // Class for kalman tracks
18
19 AliHLTKalmanTrack::AliHLTKalmanTrack()
20 {
21   fX = 0;
22
23   fMaxChi2 = 1000;
24   // Constructor
25 }
26
27 AliHLTKalmanTrack::~AliHLTKalmanTrack()
28 {
29   // Destructor
30 }
31
32 Int_t AliHLTKalmanTrack::MakeSeed(AliHLTTrack *track, AliHLTSpacePointData *points0, UInt_t pos0, Int_t slice0, AliHLTSpacePointData *points1, UInt_t pos1, Int_t slice1, AliHLTSpacePointData *points2, UInt_t pos2, Int_t slice2)
33 {
34   Float_t xyz[3];
35   xyz[0] = points0[pos0].fX;
36   xyz[1] = points0[pos0].fY;
37   AliHLTTransform::Global2LocHLT(xyz,slice0);  
38   fX = xyz[0];
39   fP0 = xyz[1];
40   fP1 = points0[pos0].fZ; 
41
42   xyz[0] = points1[pos1].fX;
43   xyz[1] = points1[pos1].fY;
44   AliHLTTransform::Global2LocHLT(xyz,slice1);
45   Float_t x2 = xyz[0];
46   Float_t y2 = xyz[1];
47   Float_t z2 = points1[pos1].fZ;
48
49   xyz[0] = points2[pos2].fX;
50   xyz[1] = points2[pos2].fY;
51   AliHLTTransform::Global2LocHLT(xyz,slice2);
52   Float_t x3 = 0;//xyz[0];
53   Float_t y3 = 0;//xyz[1];
54   Float_t z3 = 0;//points2[pos2].fZ; 
55
56
57   fP2 = f2(fX,fP0,x2,y2,x3,y3);
58   fP3 = f3(fX,fP0,x2,y2,fP1,z2);
59   if (TMath::Abs(fP3) > 1.2) return 0;
60   fP4 = f4(fX,fP0,x2,y2,x3,y3);
61   if (TMath::Abs(fP4) >= 0.0066) return 0;
62
63   Float_t sy1=points0[pos0].fSigmaY2, sz1=points0[pos0].fSigmaZ2;
64   Float_t sy2=points2[pos2].fSigmaY2, sz2=points2[pos2].fSigmaZ2;
65   //Double_t sy3=400*3./12., sy=0.1, sz=0.1;
66   Float_t sy3=25000*fP4*fP4+0.1, sy=0.1, sz=0.1;
67   
68   Float_t f40=(f4(fX,fP0+sy,x2,y2,x3,y3)-fP4)/sy;
69   Float_t f42=(f4(fX,fP0,x2,y2+sy,x3,y3)-fP4)/sy;
70   Float_t f43=(f4(fX,fP0,x2,y2,x3,y3+sy)-fP4)/sy;
71   Float_t f20=(f2(fX,fP0+sy,x2,y2,x3,y3)-fP2)/sy;
72   Float_t f22=(f2(fX,fP0,x2,y2+sy,x3,y3)-fP2)/sy;
73   Float_t f23=(f2(fX,fP0,x2,y2,x3,y3+sy)-fP2)/sy;
74   Float_t f30=(f3(fX,fP0+sy,x2,y2,fP1,z2)-fP3)/sy;
75   Float_t f31=(f3(fX,fP0,x2,y2,fP1+sz,z2)-fP3)/sz;
76   Float_t f32=(f3(fX,fP0,x2,y2+sy,fP1,z2)-fP3)/sy;
77   Float_t f34=(f3(fX,fP0,x2,y2,fP1,z2+sz)-fP3)/sz;
78
79   fC00=sy1;
80   fC10=0.;       fC11=sz1;
81   fC20=f20*sy1;  fC21=0.;       fC22=f20*sy1*f20+f22*sy2*f22+f23*sy3*f23;
82   fC30=f30*sy1;  fC31=f31*sz1;  fC32=f30*sy1*f20+f32*sy2*f22;
83   fC33=f30*sy1*f30+f31*sz1*f31+f32*sy2*f32+f34*sz2*f34;
84   fC40=f40*sy1; fC41=0.; fC42=f40*sy1*f20+f42*sy2*f22+f43*sy3*f23;
85   fC43=f30*sy1*f40+f32*sy2*f42;
86   fC44=f40*sy1*f40+f42*sy2*f42+f43*sy3*f43;
87
88   return 1;  
89
90
91 Int_t AliHLTKalmanTrack::Init(AliHLTTrack *track, AliHLTSpacePointData *points, UInt_t pos, Int_t slice)
92 {
93
94   // Can also find seed for x4 by track->CalculateHelix() and fP4 = track->GetKappa()
95   // See if it's any difference in the time it takes. ??
96   //track->CalculateHelix();
97
98   Float_t xyz[3];
99   xyz[0] = points[pos].fX;
100   xyz[1] = points[pos].fY;
101   AliHLTTransform::Global2LocHLT(xyz,slice);
102   
103   fX = xyz[0];
104
105   fP0 = xyz[1];
106   fP1 = points[pos].fZ; 
107   fP3 = track->GetTgl(); //cout << fP3; 
108   if (TMath::Abs(fP3) > 1.2) return 0; //From AliTPCtrackerMI
109
110   // The expression for fP4 is from ALICE internal note: ALICE/97-24, June 27, 1997
111   // Is this consistent with what's used in AliTPCtrack and AliTPCtracker ?? 
112   // Except for the factor 0.5, it is somewhat consistent with the calculation of Pt in AliTPCtrack.
113   // When I plot fP4, it is more consistent with offline if the factor 1/2 is included.
114   //fP4 = (-track->GetCharge()*1./(track->GetPt()/(0.0029980*AliHLTTransform::GetBField()))); 
115   fP4 = 0.5*(-track->GetCharge()*1./(track->GetPt()/(0.0029980*AliHLTTransform::GetBField()))); 
116   //cout << fP4 << endl;
117   //fP4 = 0.5*track->GetKappa();
118   if (TMath::Abs(fP4) >= 0.0066) return 0; // From AliTPCtrackerMI
119   
120   /*Float_t firstXY[2];
121   firstXY[0] = track->GetFirstPointX();
122   firstXY[1] = track->GetFirstPointY();
123   AliHLTTransform::Global2LocHLT(firstXY,slice);
124   */
125   //Float_t centerX = track->GetFirstPointX() - ((track->GetPt()/(0.0029980*AliHLTTransform::GetBField())) * TMath::Cos(track->GetPsi() + track->GetCharge() * 0.5 * 3.14159265358979323846));
126   //Float_t centerX = firstXY[0] - ((track->GetPt()/(0.0029980*AliHLTTransform::GetBField())) * TMath::Cos(track->GetPsi() + track->GetCharge() * 0.5 * 3.14159265358979323846));
127   
128   //fP2 = fP4*centerX; // Curvature times center of curvature
129   // track->GetPointPsi() is almost always zero, why??. Still it's good for seed.
130   fP2 = TMath::Sin(track->GetPointPsi());  
131
132   //cout << track->GetPt() << endl;
133   if (TMath::Abs(fP4*fX - fP2) >= 0.9) // What's this??
134     {
135       return 0;
136     }
137
138   //cout << "AliHLTKalmanTrack::Init, " << fP0 << " " << fP1 << " " << fP2 << " " << fP3 << " " << fP4 << endl;
139   //Float_t num = 12;
140
141   fC00 = points[pos].fSigmaY2;
142   fC10 = 0; fC11 = points[pos].fSigmaZ2;
143   fC20 = 0; fC21 = 0; fC22 = 5e-05;
144   fC30 = 0; fC31 = 0; fC32 = 0; fC33 = 5e-05;
145   fC40 = 0; fC41 = 0; fC42 = 0; fC43 = 0; fC44 = 5e-09;
146   /*Float_t num = 12;
147   fC00 = points[pos].fSigmaY2;
148   fC10 = 0; fC11 = points[pos].fSigmaZ2;
149   fC20 = 0.0005; fC21 = 0; fC22 = 5.5e-05;
150   fC30 = 0.0001; fC31 = 0.001;
151   fC32 = 5.5e-06; fC33 = 6.5e-05;
152   fC40 = 5e-06;
153   fC41 = 0; fC42 = 4e-07; fC43 = 4e-08; fC44 = 3e-09;*/
154   //cout << "Init: errors " << fC00 << " " << fC11 << " " << fC22 << " " << fC33 << " " << fC44 << endl;
155
156   fChisq = 0;
157
158   return 1;
159 }
160
161 Int_t AliHLTKalmanTrack::Propagate(AliHLTSpacePointData *points, UInt_t pos, Int_t slice)
162 {
163   // Propagates track to the plane of the next found cluster
164   Float_t Xold = fX; // X position for previous space point
165   //Float_t Xnew = points[pos].fX; // X position of current space point
166   //Float_t dx = Xnew - Xold; cout << Xnew << endl;
167   Float_t Yold = fP0; // Y position of old point
168   Float_t Zold = fP1; // Z position of old point
169   Float_t par2old = fP2;  
170   Float_t par3old = fP3;
171   Float_t par4old = fP4;
172   Float_t oldfC00 = fC00;
173   Float_t oldfC10 = fC10;
174   Float_t oldfC11 = fC11;
175   Float_t oldfC20 = fC20;
176   Float_t oldfC21 = fC21;
177   Float_t oldfC22 = fC22;
178   Float_t oldfC30 = fC30;
179   Float_t oldfC31 = fC31;
180   Float_t oldfC32 = fC32;
181   Float_t oldfC33 = fC33;
182   Float_t oldfC40 = fC40;
183   Float_t oldfC41 = fC41;
184   Float_t oldfC42 = fC42;
185   Float_t oldfC43 = fC43;
186   Float_t oldfC44 = fC44;
187
188   Float_t xyz[3];
189
190   xyz[0] = points[pos].fX;
191   xyz[1] = points[pos].fY;
192
193   //AliHLTTransform::Global2Local(xyz,slice);
194   AliHLTTransform::Global2LocHLT(xyz,slice);
195   
196   Float_t Xnew = xyz[0];
197   Float_t dx = Xnew - Xold; //cout << Xnew << endl;
198   
199   if (TMath::Abs(fP4*Xnew - fP2) >= 0.9) // What's this??
200     {
201       //cout << "Propagation failed! Stiff track. " << pos << endl;
202       return 0;
203     }
204   
205   // R must be approx. of the radius of the circle based on
206   // the old and new spacepoint. What then is Cold and Cnew??
207   // C has something to do with curvature at least (curvature times x).
208
209   Float_t Cold = fP4*Xold - fP2; 
210   //if (TMath::Abs(Cold) >= 1.0) Cold = 0.9;
211     /*{
212     cout << "Cold " << endl << fP4*Xnew - fP2 << endl;
213     return 0;
214     }*/
215   Float_t Rold = TMath::Sqrt(1 - Cold*Cold);
216   Float_t Cnew = fP4*Xnew - fP2; 
217   //if (TMath::Abs(Cnew) >= 1.0) Cnew = 0.9;
218     /*{
219     cout << "Cnew " << endl;
220     return 0;
221     }*/
222   Float_t Rnew = TMath::Sqrt(1 - Cnew*Cnew);
223   //if (Rold < 0.9) cout << "Cold = " << Cold << " , Rold = " << Rold << " , Cnew = " << Cnew << " , Rnew = " << Rnew << endl;
224
225   // Prediction of the y- and z- coordinate in the next plane
226   fP0 += dx*(Cold+Cnew)/(Rold+Rnew);
227   fP1 += dx*(Cold+Cnew)/(Cold*Rnew + Cnew*Rold)*fP3; 
228   //cout << "Old point " << Yold << " " << Zold << endl;
229   //cout << "Propagate " << fP0 << " " << fP1 << endl;
230   //cout << "Measured  " << points[pos].fY << " " << points[pos].fZ << endl;
231
232   // f = F - 1 //What is this??
233   // Must be the f-matrix for the prediction, as in eq 1 in ALICE Kalman paper
234   Float_t RR = Rold + Rnew;
235   Float_t CC = Cold + Cnew;
236   Float_t XX = Xold + Xnew;
237
238   Float_t f02 = -dx*(2*RR + CC*(Cold/Rold + Cnew/Rnew))/(RR*RR);
239   Float_t f04 = dx*(RR*XX + CC*(Cold*Xold/Rold + Cnew*Xnew/Rnew))/(RR*RR);
240   Float_t CR = Cold*Rnew + Cnew*Rold;
241
242   Float_t f12 = -dx*fP3*(2*CR + CC*(Cnew*(Cold/Rold)-Rold + Cold*(Cnew/Rnew)-Rnew))/(CR*CR);
243   Float_t f13 = dx*CC/CR;
244   Float_t f14 = dx*fP3*(CR*XX-CC*(Rold*Xnew-Cnew*Cold*Xold/Rold + Rnew*Xold-Cold*Cnew*Xnew/Rnew))/(CR*CR);
245
246   // b = C*ft // This? MUST BE (f*C)t = Ct*ft??, that gives the expressions under at least.
247   Float_t b00=f02*fC20 + f04*fC40;
248   Float_t b01=f12*fC20 + f14*fC40 + f13*fC30;
249   Float_t b10=f02*fC21 + f04*fC41;
250   Float_t b11=f12*fC21 + f14*fC41 + f13*fC31;
251   Float_t b20=f02*fC22 + f04*fC42;
252   Float_t b21=f12*fC22 + f14*fC42 + f13*fC32;
253   Float_t b30=f02*fC32 + f04*fC43;
254   Float_t b31=f12*fC32 + f14*fC43 + f13*fC33;
255   Float_t b40=f02*fC42 + f04*fC44;
256   Float_t b41=f12*fC42 + f14*fC44 + f13*fC43;
257
258   //a = f*b = f*C*ft
259   Float_t a00 = f02*b20 + f04*b40;
260   Float_t a01 = f02*b21 + f04*b41;
261   Float_t a11 = f12*b21 + f14*b41+f13*b31;
262
263   //F*C*Ft = C + (a + b + bt) , This is the covariance matrix, the samll t 
264   // means transform. Then F must be df/dx 
265   fC00 += a00 + 2*b00;
266   fC10 += a01 + b01 + b10;
267   fC20 += b20;
268   fC30 += b30;
269   fC40 += b40;
270   fC11 += a11 + 2*b11;
271   fC21 += b21;
272   fC31 += b31;
273   fC41 += b41;
274
275   // Multiple scattering (from AliTPCtrack::PropagateTo)
276   // Should this be included??
277   /*
278   Float_t d = TMath::Sqrt((Xold-Xnew)*(Xold-Xnew)+(Yold-fP0)*(Yold-fP0)+(Zold-fP1)*(Zold-fP1));
279   Float_t Pt = (1e-9*TMath::Abs(fP4)/fP4 + fP4 * (1000/0.299792458/4));
280   if (TMath::Abs(Pt) > 100) {
281     return 0;
282   }
283   if (TMath::Abs(Pt) < 0.01) return 0;
284   
285   Float_t p2 = (1+fP3*fP3)/(Pt*Pt);
286   Float_t beta2 = p2/(p2 +0.14*0.14);
287   Float_t theta2 = 1.0259e-6*10*10/20/(beta2*p2)*d*0.9e-3;
288   
289   Float_t ey=fP4*Xnew - fP2;
290   Float_t ez=fP3;
291   Float_t xz=fP4*ez;
292   Float_t zz1=ez*ez+1;
293   Float_t xy=fP2+ey;
294   
295   fC22 += (2*ey*ez*ez*fP2+1-ey*ey+ez*ez+fP2*fP2*ez*ez)*theta2;
296   fC32 += ez*zz1*xy*theta2;
297   fC33 += zz1*zz1*theta2;
298   fC42 += xz*ez*xy*theta2;
299   fC43 += xz*zz1*theta2;
300   fC44 += xz*xz*theta2;
301   if (TMath::Abs(beta2) >= 1) cout << "here? " << beta2 << ", " << Pt << endl;
302   
303   // Energy loss assuming track is pion
304   Float_t dE = 0.153e-3/beta2*(log(5940*beta2/(1-beta2))-beta2)*d*0.9e-3;
305   if (Xold < Xnew) dE = -dE;
306   CC = fP4;
307   fP4 *= (1 - TMath::Sqrt(p2+0.14*0.14)/p2*dE);
308   fP2 += Xnew*(fP4-CC);
309   */
310   // Update the track parameters with the measured values of the new point
311
312   Int_t value = UpdateTrack(points, pos, slice);
313
314   if (value == 0)
315     {
316       fP0 = Yold;
317       fP1 = Zold;
318       fP2 = par2old;
319       fP3 = par3old;
320       fP4 = par4old;
321       fC00 = oldfC00;
322       fC10 = oldfC10;
323       fC11 = oldfC11;
324       fC20 = oldfC20;
325       fC21 = oldfC21;
326       fC22 = oldfC22;
327       fC30 = oldfC30;
328       fC31 = oldfC31;
329       fC32 = oldfC32;
330       fC33 = oldfC33;
331       fC40 = oldfC40;
332       fC41 = oldfC41;
333       fC42 = oldfC42;
334       fC43 = oldfC43;
335       fC44 = oldfC44;
336
337       return value;
338     }
339   
340   else 
341     return value;
342   //return UpdateTrack(points, pos, slice);
343 }
344
345 Int_t AliHLTKalmanTrack::UpdateTrack(AliHLTSpacePointData *points, UInt_t pos, Int_t slice)
346 {
347   // Update the track parameters with the measured values
348   
349   Float_t xyz[3];
350
351   xyz[0] = points[pos].fX;
352   xyz[1] = points[pos].fY;
353   
354   //AliHLTTransform::Global2Local(xyz,slice);
355   AliHLTTransform::Global2LocHLT(xyz,slice);
356   
357   //fX = points[pos].fX; 
358   fX = xyz[0];
359
360   // The errors from the measurement of the spacepoint
361   Float_t sigmaY2 = points[pos].fSigmaY2;
362   Float_t sigmaZ2 = points[pos].fSigmaZ2;
363   Float_t sigmaYZ = 0;
364
365   // points[pos].fSigmaY2 and Z2 is needed in calculation of chisq. 
366   // There use realSigmaZ2(z2), because sigmaY2(Z2) are changed underneath
367   // ANother possibility is to give realSigmaY2(Z2)(YZ) after += fC00(11)10 calculations. 
368   // After all it is these that are used in chisq calculation. 
369   // NB! In that case GetChisqIncrement must be changed so fC00(11)(10) are not added twice??. 
370   // Check if it's any change in timing.
371   Float_t realSigmaY2 = sigmaY2;
372   Float_t realSigmaZ2 = sigmaZ2;
373
374   sigmaY2 += fC00;
375   sigmaZ2 += fC11;
376   sigmaYZ += fC10;
377
378   Float_t det = sigmaY2*sigmaZ2 - sigmaYZ*sigmaYZ;
379   Float_t tmp = sigmaY2;
380   sigmaY2 = sigmaZ2/det;
381   sigmaZ2 = tmp/det;
382   sigmaYZ = -sigmaYZ/det;
383
384   // Kalman gain matrix
385   Float_t k00 = fC00*sigmaY2 + fC10*sigmaYZ;
386   Float_t k01 = fC00*sigmaYZ + fC10*sigmaZ2;
387   Float_t k10 = fC10*sigmaY2 + fC11*sigmaYZ;
388   Float_t k11 = fC10*sigmaYZ + fC11*sigmaZ2;
389   Float_t k20 = fC20*sigmaY2 + fC21*sigmaYZ;
390   Float_t k21 = fC20*sigmaYZ + fC21*sigmaZ2;
391   Float_t k30 = fC30*sigmaY2 + fC31*sigmaYZ;
392   Float_t k31 = fC30*sigmaYZ + fC31*sigmaZ2;
393   Float_t k40 = fC40*sigmaY2 + fC41*sigmaYZ;
394   Float_t k41 = fC40*sigmaYZ + fC41*sigmaZ2;
395
396   // Deviation between the predicted and measured values of y and z 
397   //Float_t dy = points[pos].fY-fP0; //cout << "dy = " << dy;
398   Float_t dy = xyz[1] - fP0; //cout << dy << endl;;
399   Float_t dz = points[pos].fZ-fP1; //cout << dz << endl; 
400   //cout << "Measured " << xyz[2] << " " << points[pos].fZ << endl;
401
402   // Update of fP2 and fP4
403   Float_t cur = fP4 + k40*dy + k41*dz; 
404   Float_t eta = fP2 + k20*dy + k21*dz;
405
406   if (TMath::Abs(cur*fX - eta) >= 0.9)
407     {
408       //cout << "Update failed! Stiff track. " << pos << endl;
409       return 0;
410     }
411
412   // Filtered state vector
413   fP0 += k00*dy + k01*dz;
414   fP1 += k10*dy + k11*dz;
415   fP2 = eta;
416   fP3 += k30*dy + k31*dz; //cout << "update " << fP3 << endl;
417   fP4 = cur;
418   //cout << "AliHLTKalmanTrack::Update, " << fP0 << " " << fP1 << " " << fP2 << " " << fP3 << " " << fP4 << endl;
419   //cout << "Measured, " << points[pos].fY << " " << points[pos].fZ << endl;
420
421   Float_t c10 = fC10;
422   Float_t c20 = fC20;
423   Float_t c30 = fC30;
424   Float_t c40 = fC40;
425   Float_t c21 = fC21;
426   Float_t c31 = fC31;
427   Float_t c41 = fC41;
428
429   // Filtered covariance matrix
430   // This is how it is in AliTPCtrack::Update
431   fC00 -= k00*fC00 + k01*fC10;
432   fC10 -= k00*c10 + k01*fC11;
433   fC11 -= k10*c10 + k11*fC11;
434   fC20 -= k00*c20 + k01*c21;
435   fC21 -= k10*c20 + k11*c21;
436   fC22 -= k20*c20 + k21*c21;
437   fC30 -= k00*c30 + k01*c31;
438   fC31 -= k10*c30 + k11*c31;
439   fC32 -= k20*c30 + k21*c31;
440   fC33 -= k30*c30 + k31*c31;
441   fC40 -= k00*c40 + k01*c41;
442   fC41 -= k10*c40 + k11*c41;
443   fC42 -= k20*c40 + k21*c41;
444   fC43 -= k40*c30 + k41*c31;
445   fC44 -= k40*c40 + k41*c41;
446   
447   // Alternative filtering
448   // This is how (I think) it should be from the Kalman filter equations, and the C-matrix in Propagate ??
449   /*Float_t c00 = fC00;
450   Float_t c11 = fC11;
451   fC00 -= k00*fC00 + k01*fC10;
452   fC10 -= k10*c00 + k11*fC10;
453   fC11 -= k10*c10 + k11*fC11;
454   fC20 -= k20*c00 + k21*c10;
455   fC21 -= k20*c10 + k21*c11;
456   fC22 -= k20*c20 + k21*c21;
457   fC30 -= k30*c00 + k31*c10;
458   fC31 -= k30*c10 + k31*c11;
459   fC32 -= k30*c20 + k31*c21;
460   fC33 -= k30*c30 + k31*c31;
461   fC40 -= k40*c00 + k41*c10;
462   fC41 -= k40*c10 + k41*c11;
463   fC42 -= k40*c20 + k41*c21;
464   fC43 -= k40*c30 + k41*c31;
465   fC44 -= k40*c40 + k41*c41;*/
466
467   /*  sigmaY2 = sigmaY2*det;
468   sigmaZ2 = sigmaZ2*det;
469   sigmaYZ = sigmaYZ*det;*/
470   //cout << "AliHLTKalmanTrack::Update, Chi2 = " << GetChisq() << endl;
471   //cout << "AliKalmanTrack::Update, sigmaY2 = " << sigmaY2 << " sigmaZ2 = " << sigmaZ2 << " sigmaYZ = " << sigmaYZ << " dy = " << dy << " dz = " << dz << endl;
472
473   // Calculate increase of chisquare
474   //fChisq = GetChisq() + GetChisqIncrement(xyz[1],sigmaY2,points[pos].fZ,sigmaZ2);
475   fChisq = GetChisq() + GetChisqIncrement(xyz[1],realSigmaY2,points[pos].fZ,realSigmaZ2);
476   //fChisq = GetChisq() + GetChisqIncrement(points[pos].fY,realSigmaY2,points[pos].fZ,realSigmaZ2);
477  //cout << "fChisq = " << fChisq << endl;
478 //(dy*sigmaY2*dy + 2*sigmaYZ*dy*dz + dz*sigmaZ2*dz) / (sigmaY2*sigmaZ2 - sigmaYZ*sigmaYZ);
479   // Must at some point make an cut on chisq. Here?
480   //  if (fChisq > fMaxChi2) return 0;
481
482   return 1;
483
484
485 //Float_t AliHLTKalmanTrack::GetChisqIncrement(AliHLTSpacePointData *points, UInt_t pos)
486 Float_t AliHLTKalmanTrack::GetChisqIncrement(Float_t y, Float_t error_y, Float_t z, Float_t error_z)
487 {
488   // This function calculates a predicted chi2 increment.
489   //-----------------------------------------------------------------
490   //Float_t r00=points[pos].fSigmaY2, r01=0., r11=points[pos].fSigmaZ2;
491   Float_t r00=error_y, r01=0., r11=error_z;
492   r00+=fC00; r01+=fC10; r11+=fC11;
493
494   Double_t det=r00*r11 - r01*r01;
495   /*if (TMath::Abs(det) < 1.e-10) {
496     Int_t n=GetNumberOfClusters();
497     if (n>4) cerr<<n<<" AliKalmanTrack warning: Singular matrix !\n";
498     return 1e10;
499     }*/
500   Double_t tmp=r00; r00=r11; r11=tmp; r01=-r01;
501   
502   Double_t dy=y - fP0, dz=z - fP1;
503   //cout << det << endl;
504   //cout << "dy = " << dy << " , dz = " << dz << " , r00 = " << r00 << " , r01 = " << r01 << " r11 = " << r11 << endl;
505   return (dy*r00*dy + 2*r01*dy*dz + dz*r11*dz)/det;
506 }
507
508 Float_t AliHLTKalmanTrack::f2(Float_t x1,Float_t y1,
509                              Float_t x2,Float_t y2,
510                              Float_t x3,Float_t y3)
511 {
512   //-----------------------------------------------------------------
513   // Initial approximation of the track curvature times center of curvature
514   // Will be removed ??
515   //-----------------------------------------------------------------
516   Double_t d=(x2-x1)*(y3-y2)-(x3-x2)*(y2-y1);
517   Double_t a=0.5*((y3-y2)*(y2*y2-y1*y1+x2*x2-x1*x1)-
518                   (y2-y1)*(y3*y3-y2*y2+x3*x3-x2*x2));
519   Double_t b=0.5*((x2-x1)*(y3*y3-y2*y2+x3*x3-x2*x2)-
520                   (x3-x2)*(y2*y2-y1*y1+x2*x2-x1*x1));
521
522   Double_t xr=TMath::Abs(d/(d*x1-a)), yr=d/(d*y1-b);
523
524   return -a/(d*y1-b)*xr/sqrt(xr*xr+yr*yr);
525 }
526
527 Float_t AliHLTKalmanTrack::f3(Float_t x1,Float_t y1,
528                              Float_t x2,Float_t y2,
529                              Float_t z1,Float_t z2)
530 {
531   //-----------------------------------------------------------------
532   // Initial approximation of the tangent of the track dip angle
533   // Will be removed ??
534   //-----------------------------------------------------------------
535   return (z1 - z2)/sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2));
536 }
537
538 Float_t AliHLTKalmanTrack::f4(Float_t x1,Float_t y1,
539                              Float_t x2,Float_t y2,
540                              Float_t x3,Float_t y3)
541 {
542   //-----------------------------------------------------------------
543   // Initial approximation of the track curvature
544   // Will be removed??
545   //-----------------------------------------------------------------
546   Double_t d=(x2-x1)*(y3-y2)-(x3-x2)*(y2-y1);
547   Double_t a=0.5*((y3-y2)*(y2*y2-y1*y1+x2*x2-x1*x1)-
548                   (y2-y1)*(y3*y3-y2*y2+x3*x3-x2*x2));
549   Double_t b=0.5*((x2-x1)*(y3*y3-y2*y2+x3*x3-x2*x2)-
550                   (x3-x2)*(y2*y2-y1*y1+x2*x2-x1*x1));
551
552   Double_t xr=TMath::Abs(d/(d*x1-a)), yr=d/(d*y1-b);
553
554   return -xr*yr/sqrt(xr*xr+yr*yr);
555 }
556
557 void AliHLTKalmanTrack::Set(AliHLTKalmanTrack *track)
558 {
559
560   AliHLTKalmanTrack *tpt = (AliHLTKalmanTrack*)track;
561   SetX0(tpt->GetX0());
562   SetX1(tpt->GetX1());
563   SetX2(tpt->GetX2());
564   SetX3(tpt->GetX3());
565   SetX4(tpt->GetX4());
566
567   SetC0(tpt->GetC0());
568   SetC1(tpt->GetC1());
569   SetC2(tpt->GetC2());
570   SetC3(tpt->GetC3());
571   SetC4(tpt->GetC4());
572   SetC5(tpt->GetC5());
573   SetC6(tpt->GetC6());
574   SetC7(tpt->GetC7());
575   SetC8(tpt->GetC8());
576   SetC9(tpt->GetC9());
577   SetC10(tpt->GetC10());
578   SetC11(tpt->GetC11());
579   SetC12(tpt->GetC12());
580   SetC13(tpt->GetC13());
581   SetC14(tpt->GetC14());
582
583   SetNHits(tpt->GetNHits());
584 }
585
586 Int_t AliHLTKalmanTrack::PropagateOfflineTrack(Double_t x, Double_t y, Double_t z, Double_t ey, Double_t ez)
587 {
588   // Propagates track to the plane of the next found cluster
589   Float_t Xold = fX; // X position for previous space point
590   Float_t Xnew = x; // X position of current space point
591   Float_t dx = Xnew - Xold; //cout << dx << endl;
592   Float_t Yold = fP0; // Y position of old point
593   Float_t Zold = fP1; // Z position of old point
594   Float_t par2old = fP2;
595   Float_t par3old = fP3;
596   Float_t par4old = fP4;
597   Float_t oldfC00 = fC00;
598   Float_t oldfC10 = fC10;
599   Float_t oldfC11 = fC11;
600   Float_t oldfC20 = fC20;
601   Float_t oldfC21 = fC21;
602   Float_t oldfC22 = fC22;
603   Float_t oldfC30 = fC30;
604   Float_t oldfC31 = fC31;
605   Float_t oldfC32 = fC32;
606   Float_t oldfC33 = fC33;
607   Float_t oldfC40 = fC40;
608   Float_t oldfC41 = fC41;
609   Float_t oldfC42 = fC42;
610   Float_t oldfC43 = fC43;
611   Float_t oldfC44 = fC44;
612
613   if (TMath::Abs(fP4*Xnew - fP2) >= 0.9) // What's this??
614     {
615       //cout << "Propagation failed! Stiff track. " << pos << endl;
616       return 0;
617     }
618
619   if (TMath::Abs(dx) > 5) return 0;
620
621   /*if (TMath::Abs(fP4*Xold - fP2) >= 0.9) // What's this??
622     {
623       return 0;
624       }*/
625
626   // R must be approx. of the radius of the circle based on
627   // the old and new spacepoint. What then is Cod and Cnew??
628   // C has something to do with curvature at least.
629   
630   Float_t Cold = fP4*Xold - fP2;
631
632   //if (TMath::Abs(Cold) >= 1.0) return 0;
633   /*{
634     cout << "Cold " << endl << fP4*Xnew - fP2 << endl;
635     return 0;
636     }*/
637   Float_t Rold = TMath::Sqrt(1 - Cold*Cold);
638   Float_t Cnew = fP4*Xnew - fP2; 
639
640   //if (TMath::Abs(Cnew) >= 1.0) Cnew = 0.9;
641     /*{
642     cout << "Cnew " << endl;
643     return 0;
644     }*/
645   Float_t Rnew = TMath::Sqrt(1 - Cnew*Cnew);
646   //if (Rold < 0.9) 
647   //cout << "Cold = " << Cold << " , Rold = " << Rold << " , Cnew = " << Cnew << " , Rnew = " << Rnew << endl;  
648   // Prediction of the y- and z- coordinate in the next plane
649   fP0 += dx*(Cold+Cnew)/(Rold+Rnew);
650   fP1 += dx*(Cold+Cnew)/(Cold*Rnew + Cnew*Rold)*fP3;
651   //cout << "Old point " << Yold << " " << Zold << endl;
652   //cout << "Propagate " << fP0 << " " << fP1 << endl;
653   //cout << "Measured  " << points[pos].fY << " " << points[pos].fZ << endl;
654
655   fX = Xnew;
656
657   // f = F - 1 //What is this??
658   // Must be the f-matrix for the prediction, as in eq 1 in ALICE Kalman paper
659   Float_t RR = Rold + Rnew;
660   Float_t CC = Cold + Cnew;
661   Float_t XX = Xold + Xnew;
662
663   Float_t f02 = -dx*(2*RR + CC*(Cold/Rold + Cnew/Rnew))/(RR*RR);
664   Float_t f04 = dx*(RR*XX + CC*(Cold*Xold/Rold + Cnew*Xnew/Rnew))/(RR*RR);
665   Float_t CR = Cold*Rnew + Cnew*Rold;
666   Float_t f12 = -dx*fP3*(2*CR + CC*(Cnew*(Cold/Rold)-Rold + Cold*(Cnew/Rnew)-Rnew))/(CR*CR);
667   Float_t f13 = dx*CC/CR;
668   Float_t f14 = dx*fP3*(CR*XX-CC*(Rold*Xnew-Cnew*Cold*Xold/Rold + Rnew*Xold-Cold*Cnew*Xnew/Rnew))/(CR*CR);
669
670   // b = C*ft // This?
671   Float_t b00=f02*fC20 + f04*fC40;
672   Float_t b01=f12*fC20 + f14*fC40 + f13*fC30;
673   Float_t b10=f02*fC21 + f04*fC41;
674   Float_t b11=f12*fC21 + f14*fC41 + f13*fC31;
675   Float_t b20=f02*fC22 + f04*fC42;
676   Float_t b21=f12*fC22 + f14*fC42 + f13*fC32;
677   Float_t b30=f02*fC32 + f04*fC43;
678   Float_t b31=f12*fC32 + f14*fC43 + f13*fC33;
679   Float_t b40=f02*fC42 + f04*fC44;
680   Float_t b41=f12*fC42 + f14*fC44 + f13*fC43;
681
682   //a = f*b = f*C*ft
683   Float_t a00 = f02*b20 + f04*b40;
684   Float_t a01 = f02*b21 + f04*b41;
685   Float_t a11 = f12*b21 + f14*b41+f13*b31;
686
687   //F*C*Ft = C + (a + b + bt) /This is the covariance matrix, the samll t
688   // means transform. Then F must be df/dx
689   fC00 += a00 + 2*b00;
690   fC10 += a01 + b01 + b10;
691   fC20 += b20;
692   fC30 += b30;
693   fC40 += b40;
694   fC11 += a11 + 2*b11;
695   fC21 += b21;
696   fC31 += b31;
697   fC41 += b41;
698
699   // Update the track parameters with the measured values of the new point
700   Int_t value = UpdateOfflineTrack(x,y,z,ey,ez);
701   
702   if (value == 0)
703     {
704       fP0 = Yold;
705       fP1 = Zold;
706       fP2 = par2old;
707       fP3 = par3old;
708       fP4 = par4old;
709       fC00 = oldfC00;
710       fC10 = oldfC10;
711       fC11 = oldfC11;
712       fC20 = oldfC20;
713       fC21 = oldfC21;
714       fC22 = oldfC22;
715       fC30 = oldfC30;
716       fC31 = oldfC31;
717       fC32 = oldfC32;
718       fC33 = oldfC33;
719       fC40 = oldfC40;
720       fC41 = oldfC41;
721       fC42 = oldfC42;
722       fC43 = oldfC43;
723       fC44 = oldfC44;
724       
725       return value;
726       }
727   
728   else
729     return value;
730   //return 1;
731   //return UpdateTrack(points, pos);
732
733 }
734
735 Int_t AliHLTKalmanTrack::UpdateOfflineTrack(Double_t x, Double_t y, Double_t z, Double_t ey, Double_t ez)
736 {
737   // Update the track parameters with the measured values
738   //fX = x;
739
740   // The errors from the measurement of the spacepoint
741   Float_t sigmaY2 = ey;
742   Float_t sigmaZ2 = ez;
743   Float_t sigmaYZ = 0;
744  
745   sigmaY2 += fC00;
746   sigmaZ2 += fC11;
747   sigmaYZ += fC10;
748
749   Float_t det = sigmaY2*sigmaZ2 - sigmaYZ*sigmaYZ;
750   Float_t tmp = sigmaY2;
751   sigmaY2 = sigmaZ2/det;
752   sigmaZ2 = tmp/det;
753   sigmaYZ = -sigmaYZ/det;
754  
755   // Kalman gain matrix
756   Float_t k00 = fC00*sigmaY2 + fC10*sigmaYZ, k01 = fC00*sigmaYZ + fC10*sigmaZ2;
757   Float_t k10 = fC10*sigmaY2 + fC11*sigmaYZ, k11 = fC10*sigmaYZ + fC11*sigmaZ2;
758   Float_t k20 = fC20*sigmaY2 + fC21*sigmaYZ, k21 = fC20*sigmaYZ + fC21*sigmaZ2;
759   Float_t k30 = fC30*sigmaY2 + fC31*sigmaYZ, k31 = fC30*sigmaYZ + fC31*sigmaZ2;
760   Float_t k40 = fC40*sigmaY2 + fC41*sigmaYZ, k41 = fC40*sigmaYZ + fC41*sigmaZ2;
761   //cout << "x = " << fX << endl;
762   // Deviation between the predicted and measured values of y and z
763   Float_t dy = y-fP0; //cout << dy << endl;
764   Float_t dz = z-fP1; //cout << ", dz = " << dz << endl;
765   //cout << "Measured " << points[pos].fY << " " << points[pos].fZ << endl;
766   
767   // Prediction of fP2 and fP4
768   Float_t cur = fP4 + k40*dy + k41*dz;
769   Float_t eta = fP2 + k20*dy + k21*dz;
770   
771   if (TMath::Abs(cur*fX - eta) >= 0.9)
772     {
773       //cout << "Update failed! Stiff track. " << pos << endl;
774       return 0;
775     }
776
777   // Filtered state vector
778   fP0 += k00*dy + k01*dz;
779   fP1 += k10*dy + k11*dz;
780   fP2 = eta;
781   fP3 += k30*dy + k31*dz; //cout << "update " << fP3 << endl;
782   fP4 = cur;
783   //cout << "AliHLTKalmanTrack::Update, " << fP0 << " " << fP1 << " " << fP2 << " " << fP3 << " " << fP4 << endl;
784   //cout << "Measured, " << points[pos].fY << " " << points[pos].fZ << endl;
785   
786   Float_t c01 = fC10;
787   Float_t c02 = fC20;
788   Float_t c03 = fC30;
789   Float_t c04 = fC40;
790   Float_t c12 = fC21;
791   Float_t c13 = fC31;
792   Float_t c14 = fC41;
793   
794   // Filtered covariance matrix
795   fC00 -= k00*fC00 + k01*fC10; fC10 -= k00*c01 + k01*fC11;
796   fC20 -= k00*c02 + k01*c12;   fC30 -= k00*c03 + k01*c13;
797   fC40 -= k00*c04 + k01*c14;
798
799   fC11 -= k10*c01 + k11*fC11;
800   fC21 -= k10*c02 + k11*c12;   fC31 -= k10*c03 + k11*c13;
801   fC41 -= k10*c04 + k11*c14;
802
803   fC22 -= k20*c02 + k21*c12;   fC32 -= k20*c03 + k21*c13;
804   fC42 -= k20*c04 + k21*c14;
805
806   fC33 -= k30*c03 + k31*c13;
807   fC43 -= k40*c03 + k41*c13;
808
809   fC44 -= k40*c04 + k41*c14;
810
811   //cout << "AliHLTKalmanTrack::Update, error " << fC00 << " " << fC11 << " " << fC22 << " " << fC33 << " " << fC44 << endl;
812
813   sigmaY2 = sigmaY2*det;
814   sigmaZ2 = sigmaZ2*det;
815   sigmaYZ = sigmaYZ*det;
816   //cout << "AliHLTKalmanTrack::Update, Chi2 = " << GetChisq() << endl;
817
818   fChisq = GetChisq() + GetChisqIncrementOfflineTrack(y,z,ey,ez);
819   //cout << "fChisq = " << fChisq << endl;
820
821   // Must at some point make an cut on chisq. Here?
822   //if (fChisq > fMaxChi2) return 0;
823   
824   return 1;
825   
826 }
827
828 Float_t AliHLTKalmanTrack::GetChisqIncrementOfflineTrack(Double_t y, Double_t z, Double_t ey, Double_t ez)
829 {
830   // This function calculates a predicted chi2 increment.
831   //-----------------------------------------------------------------
832   Float_t r00=ey, r01=0., r11=ez;
833   r00+=fC00; r01+=fC10; r11+=fC11;
834
835   Double_t det=r00*r11 - r01*r01;
836   /*if (TMath::Abs(det) < 1.e-10) {
837     Int_t n=GetNumberOfClusters();
838     if (n>4) cerr<<n<<" AliKalmanTrack warning: Singular matrix !\n";
839     return 1e10;
840     }*/
841   Double_t tmp=r00; r00=r11; r11=tmp; r01=-r01;
842   
843   Double_t dy=y - fP0, dz=z - fP1;
844   //cout << "AliTPCtrack::GetPredictedChi2, r00 = " << r00 << " r11 = " << r11 << " ro1 = " << r01 << " dy = " << dy << " dz = " << dz << endl;
845   return (dy*r00*dy + 2*r01*dy*dz + dz*r11*dz)/det;
846 }