]>
Commit | Line | Data |
---|---|---|
9c4c8863 | 1 | /************************************************************************** |
2 | * Copyright(c) 1998-1999, ALICE Experiment at CERN, All rights reserved. * | |
3 | * * | |
4 | * Author: The ALICE Off-line Project. * | |
5 | * Contributors are mentioned in the code where appropriate. * | |
6 | * * | |
7 | * Permission to use, copy, modify and distribute this software and its * | |
8 | * documentation strictly for non-commercial purposes is hereby granted * | |
9 | * without fee, provided that the above copyright notice appears in all * | |
10 | * copies and that both the copyright notice and this permission notice * | |
11 | * appear in the supporting documentation. The authors make no claims * | |
12 | * about the suitability of this software for any purpose. It is * | |
13 | * provided "as is" without express or implied warranty. * | |
14 | **************************************************************************/ | |
15 | ||
16 | /////////////////////////////////////////////////////////////////////////////// | |
17 | // | |
18 | // Kalman-Filter-like fit | |
19 | // to a straight-line crossing a set of arbitrarily oriented planes. | |
20 | // The resulting line is given by the equation: | |
21 | // (x-x0)/vx = (y-y0)/1 = (z-z0)/vz | |
22 | // Parameters of the fit are: | |
23 | // x0,y0,z0 (a point on the line) and | |
24 | // vx,1,vz (a vector collinear with the line) | |
25 | // | |
26 | // LIMITATION: The line must not be perpendicular to the Y axis. | |
27 | // | |
28 | // Origin: Iouri Belikov, CERN, Jouri.Belikov@cern.ch | |
29 | // | |
30 | ////////////////////////////////////////////////////////////////////////////// | |
31 | ||
32 | #include <TMatrixD.h> | |
33 | #include <TMatrixDSym.h> | |
34 | ||
b7a98e0a | 35 | #include "AliLog.h" |
9c4c8863 | 36 | #include "AliTrackFitterKalman.h" |
37 | ||
38 | ClassImp(AliTrackFitterKalman) | |
39 | ||
40 | const Double_t AliTrackFitterKalman::fgkMaxChi2=33.; | |
41 | ||
42 | AliTrackFitterKalman:: | |
43 | AliTrackFitterKalman(AliTrackPointArray *array, Bool_t owner): | |
44 | AliTrackFitter(array,owner), | |
57c8a1c2 | 45 | fMaxChi2(fgkMaxChi2), |
46 | fSeed(kFALSE) | |
9c4c8863 | 47 | { |
48 | // | |
49 | // Constructor | |
50 | // | |
51 | } | |
52 | ||
57c8a1c2 | 53 | Bool_t AliTrackFitterKalman::Begin(Int_t first, Int_t last) { |
54 | // | |
55 | // Make a seed out of the track points with the indices "first" and "last". | |
56 | // This is the "default" seed. | |
57 | // | |
58 | if (fSeed) return kTRUE; | |
bead9796 | 59 | fPoints->Sort(); |
57c8a1c2 | 60 | AliTrackPoint fp,lp; |
61 | fPoints->GetPoint(fp,first); fPoints->GetPoint(lp,last); | |
62 | return MakeSeed(&fp,&lp); | |
63 | } | |
64 | ||
65 | ||
66 | Bool_t AliTrackFitterKalman:: | |
9c4c8863 | 67 | MakeSeed(const AliTrackPoint *p0, const AliTrackPoint *p1) { |
68 | // | |
69 | // Make a seed out of the two track points | |
70 | // | |
71 | Double_t x0=p0->GetX(), y0=p0->GetY(), z0=p0->GetZ(); | |
72 | Double_t dx=p1->GetX()-x0, dy=p1->GetY()-y0, dz=p1->GetZ()-z0; | |
b7a98e0a | 73 | if (dy==0.) { |
74 | AliError("Seeds perpendicular to Y axis are not allowed !"); | |
75 | return kFALSE; | |
76 | } | |
9c4c8863 | 77 | Double_t vx=dx/dy; |
78 | Double_t vz=dz/dy; | |
79 | Double_t par[5]={x0,y0,z0,vx,vz}; | |
80 | ||
81 | const Float_t *cv0=p0->GetCov(); | |
82 | const Float_t *cv1=p1->GetCov(); | |
9c4c8863 | 83 | Double_t rdy2=(cv0[3]+cv1[3])/dy/dy; |
b7a98e0a | 84 | Double_t svx2=(cv0[0]+cv1[0])/dy/dy + vx*vx*rdy2; |
85 | Double_t svz2=(cv0[5]+cv1[5])/dy/dy + vz*vz*rdy2; | |
9c4c8863 | 86 | Double_t cov[15]={ |
87 | cv0[0], | |
88 | cv0[1],cv0[3], | |
89 | cv0[2],cv0[4],cv0[5], | |
90 | 0.,0.,0.,svx2, | |
91 | 0.,0.,0.,0.,svz2 | |
92 | }; | |
93 | ||
94 | SetSeed(par,cov); | |
57c8a1c2 | 95 | |
96 | return kTRUE; | |
9c4c8863 | 97 | } |
98 | ||
99 | Bool_t AliTrackFitterKalman::AddPoint(const AliTrackPoint *p) | |
100 | { | |
101 | // | |
102 | // Add a point to the fit | |
103 | // | |
104 | ||
105 | if (!Propagate(p)) return kFALSE; | |
106 | Double_t chi2=GetPredictedChi2(p); | |
107 | if (chi2>fMaxChi2) return kFALSE; | |
108 | if (!Update(p,chi2)) return kFALSE; | |
109 | ||
110 | return kTRUE; | |
111 | } | |
112 | ||
113 | ||
114 | Double_t AliTrackFitterKalman::GetPredictedChi2(const AliTrackPoint *p) const { | |
115 | // | |
116 | // Calculate the predicted chi2 increment. | |
117 | // | |
118 | ||
119 | Float_t txyz[3]={GetParam()[0],GetParam()[1],GetParam()[2]}; | |
120 | TMatrixDSym &cv=*fCov; | |
121 | Float_t tcov[6]={ | |
122 | cv(0,0),cv(1,0),cv(2,0), | |
123 | cv(1,1),cv(2,1), | |
124 | cv(2,2) | |
125 | }; | |
126 | AliTrackPoint tp(txyz,tcov,p->GetVolumeID()); | |
127 | ||
128 | Double_t chi2=p->GetResidual(tp,kTRUE); | |
129 | ||
130 | return chi2; | |
131 | } | |
132 | ||
133 | ||
134 | Bool_t AliTrackFitterKalman::Propagate(const AliTrackPoint *p) { | |
135 | // | |
136 | // Propagate the track towards the measured point "p" | |
137 | // | |
138 | ||
139 | TMatrixDSym &cv=*fCov; | |
140 | Double_t s=p->GetY() - fParams[1]; | |
141 | Double_t sig2=s*s/12.; | |
142 | ||
143 | Double_t vx=fParams[3], vz=fParams[4]; | |
144 | fParams[0] += s*vx; | |
145 | fParams[1] += s; | |
146 | fParams[2] += s*vz; | |
147 | ||
148 | Double_t | |
149 | c00 = cv(0,0) + 2*s*cv(3,0) + s*s*cv(3,3) + vx*vx*sig2, | |
150 | ||
151 | c10 = cv(1,0) + s*cv(1,3) + vx*sig2, c11=cv(1,1) + sig2, | |
152 | ||
153 | c20 = cv(2,0) + s*(cv(4,0) + cv(2,3)) + s*s*cv(4,3) + vx*vz*sig2, | |
154 | c21 = cv(2,1) + s*cv(4,1) + vz*sig2, | |
155 | c22 = cv(2,2) + 2*s*cv(4,2) + s*s*cv(4,4) + vz*vz*sig2, | |
156 | ||
157 | c30 = cv(3,0) + s*cv(3,3), c31 = cv(3,1), | |
158 | c32 = cv(3,2) + s*cv(3,4), c33 = cv(3,3), | |
159 | ||
160 | c40 = cv(4,0) + s*cv(4,3), c41 = cv(4,1), | |
161 | c42 = cv(4,2) + s*cv(4,4), c43 = cv(4,3), c44 = cv(4,4); | |
162 | ||
163 | ||
164 | cv(0,0)=c00; cv(0,1)=c10; cv(0,2)=c20; cv(0,3)=c30; cv(0,4)=c40; | |
165 | cv(1,0)=c10; cv(1,1)=c11; cv(1,2)=c21; cv(1,3)=c31; cv(1,4)=c41; | |
166 | cv(2,0)=c20; cv(2,1)=c21; cv(2,2)=c22; cv(2,3)=c32; cv(2,4)=c42; | |
167 | cv(3,0)=c30; cv(3,1)=c31; cv(3,2)=c32; cv(3,3)=c33; cv(3,4)=c43; | |
168 | cv(4,0)=c40; cv(4,1)=c41; cv(4,2)=c42; cv(4,3)=c43; cv(4,4)=c44; | |
169 | ||
170 | return kTRUE; | |
171 | } | |
172 | ||
173 | Bool_t AliTrackFitterKalman::Update(const AliTrackPoint *p, Double_t chi2) { | |
174 | // | |
175 | // Update the track params using the measured point "p" | |
176 | // | |
177 | ||
178 | TMatrixDSym &c=*fCov; | |
179 | const Float_t *cov=p->GetCov(); | |
180 | ||
181 | TMatrixDSym v(3); | |
182 | v(0,0)=cov[0]+c(0,0); v(0,1)=cov[1]+c(0,1); v(0,2)=cov[2]+c(0,2); | |
183 | v(1,0)=cov[1]+c(1,0); v(1,1)=cov[3]+c(1,1); v(1,2)=cov[4]+c(1,2); | |
184 | v(2,0)=cov[2]+c(2,0); v(2,1)=cov[4]+c(2,1); v(2,2)=cov[5]+c(2,2); | |
2bece746 | 185 | if (TMath::Abs(v.Determinant()) < 1.e-33) return kFALSE; |
9c4c8863 | 186 | v.Invert(); |
187 | ||
188 | TMatrixD ch(5,3); | |
189 | ch(0,0)=c(0,0); ch(0,1)=c(0,1); ch(0,2)=c(0,2); | |
190 | ch(1,0)=c(1,0); ch(1,1)=c(1,1); ch(1,2)=c(1,2); | |
191 | ch(2,0)=c(2,0); ch(2,1)=c(2,1); ch(2,2)=c(2,2); | |
192 | ch(3,0)=c(3,0); ch(3,1)=c(3,1); ch(3,2)=c(3,2); | |
193 | ch(4,0)=c(4,0); ch(4,1)=c(4,1); ch(4,2)=c(4,2); | |
194 | ||
195 | TMatrixD k(ch,TMatrixD::kMult,v); | |
196 | ||
197 | TMatrixD d(3,1); | |
198 | d(0,0) = p->GetX() - fParams[0]; | |
199 | d(1,0) = p->GetY() - fParams[1]; | |
200 | d(2,0) = p->GetZ() - fParams[2]; | |
201 | ||
202 | TMatrixD x(k,TMatrixD::kMult,d); | |
203 | ||
204 | fParams[0]+=x(0,0); | |
205 | fParams[1]+=x(1,0); | |
206 | fParams[2]+=x(2,0); | |
207 | fParams[3]+=x(3,0); | |
208 | fParams[4]+=x(4,0); | |
209 | ||
210 | TMatrixD hc(3,5); | |
211 | hc(0,0)=c(0,0);hc(0,1)=c(0,1);hc(0,2)=c(0,2);hc(0,3)=c(0,3);hc(0,4)=c(0,4); | |
212 | hc(1,0)=c(1,0);hc(1,1)=c(1,1);hc(1,2)=c(1,2);hc(1,3)=c(1,3);hc(1,4)=c(1,4); | |
213 | hc(2,0)=c(2,0);hc(2,1)=c(2,1);hc(2,2)=c(2,2);hc(2,3)=c(2,3);hc(2,4)=c(2,4); | |
214 | ||
215 | TMatrixD s(k,TMatrixD::kMult,hc); | |
216 | ||
217 | c(0,0)-=s(0,0);c(0,1)-=s(0,1);c(0,2)-=s(0,2);c(0,3)-=s(0,3);c(0,4)-=s(0,4); | |
218 | c(1,0)-=s(1,0);c(1,1)-=s(1,1);c(1,2)-=s(1,2);c(1,3)-=s(1,3);c(1,4)-=s(1,4); | |
219 | c(2,0)-=s(2,0);c(2,1)-=s(2,1);c(2,2)-=s(2,2);c(2,3)-=s(2,3);c(2,4)-=s(2,4); | |
220 | c(3,0)-=s(3,0);c(3,1)-=s(3,1);c(3,2)-=s(3,2);c(3,3)-=s(3,3);c(3,4)-=s(3,4); | |
221 | c(4,0)-=s(4,0);c(4,1)-=s(4,1);c(4,2)-=s(4,2);c(4,3)-=s(4,3);c(4,4)-=s(4,4); | |
222 | ||
223 | fChi2 += chi2; | |
224 | fNdf += 2; | |
225 | ||
226 | return kTRUE; | |
227 | } | |
228 | ||
229 | //_____________________________________________________________________________ | |
230 | Bool_t AliTrackFitterKalman::GetPCA(const AliTrackPoint &p, AliTrackPoint &i) const | |
231 | { | |
232 | // | |
233 | // Get the intersection point "i" between the track and the plane | |
234 | // the point "p" belongs to. | |
235 | // | |
236 | ||
237 | TMatrixD t(3,1); | |
238 | Double_t s=p.GetY() - fParams[1]; | |
239 | Double_t vx=fParams[3], vz=fParams[4]; | |
240 | t(0,0) = fParams[0] + s*vx; | |
241 | t(1,0) = fParams[1] + s; | |
242 | t(2,0) = fParams[2] + s*vz; | |
243 | ||
244 | TMatrixDSym tC(3); | |
245 | { | |
246 | Double_t sig2=s*s/12.; | |
93a7cb17 | 247 | |
248 | tC(0,0) = vx*vx*sig2; | |
249 | tC(1,0) = vx*sig2; | |
250 | tC(1,1) = sig2; | |
251 | tC(2,0) = vx*vz*sig2; | |
252 | tC(2,1) = vz*sig2; | |
253 | tC(2,2) = vz*vz*sig2; | |
9c4c8863 | 254 | |
255 | tC(0,1) = tC(1,0); tC(0,2) = tC(2,0); | |
256 | tC(1,2) = tC(2,1); | |
257 | } | |
258 | ||
259 | TMatrixD m(3,1); | |
260 | m(0,0)=p.GetX(); | |
261 | m(1,0)=p.GetY(); | |
262 | m(2,0)=p.GetZ(); | |
263 | ||
264 | TMatrixDSym mC(3); | |
265 | { | |
266 | const Float_t *cv=p.GetCov(); | |
267 | mC(0,0)=cv[0]; mC(0,1)=cv[1]; mC(0,2)=cv[2]; | |
268 | mC(1,0)=cv[1]; mC(1,1)=cv[3]; mC(1,2)=cv[4]; | |
269 | mC(2,0)=cv[2]; mC(2,1)=cv[4]; mC(2,2)=cv[5]; | |
270 | } | |
271 | ||
272 | TMatrixDSym tmW(tC); | |
273 | tmW+=mC; | |
2bece746 | 274 | if (TMath::Abs(tmW.Determinant()) < 1.e-33) return kFALSE; |
9c4c8863 | 275 | tmW.Invert(); |
276 | ||
11721007 | 277 | TMatrixD mW(tC,TMatrixD::kMult,tmW); |
278 | TMatrixD tW(mC,TMatrixD::kMult,tmW); | |
9c4c8863 | 279 | |
280 | TMatrixD mi(mW,TMatrixD::kMult,m); | |
281 | TMatrixD ti(tW,TMatrixD::kMult,t); | |
282 | ti+=mi; | |
283 | ||
284 | TMatrixD iC(tC,TMatrixD::kMult,tmW); | |
285 | iC*=mC; | |
21404e08 | 286 | Double_t sig2=1.; // Releasing the matrix by 1 cm along the track direction |
287 | iC(0,0) += vx*vx*sig2; | |
288 | iC(0,1) += vx*sig2; | |
289 | iC(1,1) += sig2; | |
290 | iC(0,2) += vx*vz*sig2; | |
291 | iC(1,2) += vz*sig2; | |
292 | iC(2,2) += vz*vz*sig2; | |
293 | ||
9c4c8863 | 294 | |
295 | Float_t cov[6]={ | |
296 | iC(0,0), iC(0,1), iC(0,2), | |
297 | iC(1,1), iC(1,2), | |
298 | iC(2,2) | |
299 | }; | |
300 | i.SetXYZ(ti(0,0),ti(1,0),ti(2,0),cov); | |
301 | UShort_t id=p.GetVolumeID(); | |
302 | i.SetVolumeID(id); | |
303 | ||
304 | return kTRUE; | |
305 | } | |
306 | ||
307 | ||
308 | //_____________________________________________________________________________ | |
309 | void | |
310 | AliTrackFitterKalman::SetSeed(const Double_t par[5], const Double_t cov[15]) { | |
311 | // | |
312 | // Set the initial approximation for the track parameters | |
313 | // | |
314 | for (Int_t i=0; i<5; i++) fParams[i]=par[i]; | |
315 | fParams[5]=0.; | |
316 | ||
317 | delete fCov; | |
318 | fCov=new TMatrixDSym(5); | |
319 | TMatrixDSym &cv=*fCov; | |
320 | ||
321 | cv(0,0)=cov[0 ]; | |
322 | cv(1,0)=cov[1 ]; cv(1,1)=cov[2 ]; | |
323 | cv(2,0)=cov[3 ]; cv(2,1)=cov[4 ]; cv(2,2)=cov[5 ]; | |
324 | cv(3,0)=cov[6 ]; cv(3,1)=cov[7 ]; cv(3,2)=cov[8 ]; cv(3,3)=cov[9 ]; | |
325 | cv(4,0)=cov[10]; cv(4,1)=cov[11]; cv(4,2)=cov[12]; cv(4,3)=cov[13]; cv(4,4)=cov[14]; | |
326 | ||
327 | cv(0,1)=cv(1,0); | |
328 | cv(0,2)=cv(2,0); cv(1,2)=cv(2,1); | |
329 | cv(0,3)=cv(3,0); cv(1,3)=cv(3,1); cv(2,3)=cv(3,2); | |
330 | cv(0,4)=cv(4,0); cv(1,4)=cv(4,1); cv(2,4)=cv(4,2); cv(3,4)=cv(4,3); | |
331 | ||
57c8a1c2 | 332 | fSeed=kTRUE; |
9c4c8863 | 333 | } |