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