Updates in calibration objects handling, new calibration objects from noise/pulser...
[u/mrichter/AliRoot.git] / STEER / AliTrackFitterKalman.cxx
CommitLineData
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
38ClassImp(AliTrackFitterKalman)
39
40const Double_t AliTrackFitterKalman::fgkMaxChi2=33.;
41
42AliTrackFitterKalman::
43AliTrackFitterKalman(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 53Bool_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;
59 AliTrackPoint fp,lp;
60 fPoints->GetPoint(fp,first); fPoints->GetPoint(lp,last);
61 return MakeSeed(&fp,&lp);
62}
63
64
65Bool_t AliTrackFitterKalman::
9c4c8863 66MakeSeed(const AliTrackPoint *p0, const AliTrackPoint *p1) {
67 //
68 // Make a seed out of the two track points
69 //
70 Double_t x0=p0->GetX(), y0=p0->GetY(), z0=p0->GetZ();
71 Double_t dx=p1->GetX()-x0, dy=p1->GetY()-y0, dz=p1->GetZ()-z0;
b7a98e0a 72 if (dy==0.) {
73 AliError("Seeds perpendicular to Y axis are not allowed !");
74 return kFALSE;
75 }
9c4c8863 76 Double_t vx=dx/dy;
77 Double_t vz=dz/dy;
78 Double_t par[5]={x0,y0,z0,vx,vz};
79
80 const Float_t *cv0=p0->GetCov();
81 const Float_t *cv1=p1->GetCov();
9c4c8863 82 Double_t rdy2=(cv0[3]+cv1[3])/dy/dy;
b7a98e0a 83 Double_t svx2=(cv0[0]+cv1[0])/dy/dy + vx*vx*rdy2;
84 Double_t svz2=(cv0[5]+cv1[5])/dy/dy + vz*vz*rdy2;
9c4c8863 85 Double_t cov[15]={
86 cv0[0],
87 cv0[1],cv0[3],
88 cv0[2],cv0[4],cv0[5],
89 0.,0.,0.,svx2,
90 0.,0.,0.,0.,svz2
91 };
92
93 SetSeed(par,cov);
57c8a1c2 94
95 return kTRUE;
9c4c8863 96}
97
98Bool_t AliTrackFitterKalman::AddPoint(const AliTrackPoint *p)
99{
100 //
101 // Add a point to the fit
102 //
103
104 if (!Propagate(p)) return kFALSE;
105 Double_t chi2=GetPredictedChi2(p);
106 if (chi2>fMaxChi2) return kFALSE;
107 if (!Update(p,chi2)) return kFALSE;
108
109 return kTRUE;
110}
111
112
113Double_t AliTrackFitterKalman::GetPredictedChi2(const AliTrackPoint *p) const {
114 //
115 // Calculate the predicted chi2 increment.
116 //
117
118 Float_t txyz[3]={GetParam()[0],GetParam()[1],GetParam()[2]};
119 TMatrixDSym &cv=*fCov;
120 Float_t tcov[6]={
121 cv(0,0),cv(1,0),cv(2,0),
122 cv(1,1),cv(2,1),
123 cv(2,2)
124 };
125 AliTrackPoint tp(txyz,tcov,p->GetVolumeID());
126
127 Double_t chi2=p->GetResidual(tp,kTRUE);
128
129 return chi2;
130}
131
132
133Bool_t AliTrackFitterKalman::Propagate(const AliTrackPoint *p) {
134 //
135 // Propagate the track towards the measured point "p"
136 //
137
138 TMatrixDSym &cv=*fCov;
139 Double_t s=p->GetY() - fParams[1];
140 Double_t sig2=s*s/12.;
141
142 Double_t vx=fParams[3], vz=fParams[4];
143 fParams[0] += s*vx;
144 fParams[1] += s;
145 fParams[2] += s*vz;
146
147 Double_t
148 c00 = cv(0,0) + 2*s*cv(3,0) + s*s*cv(3,3) + vx*vx*sig2,
149
150 c10 = cv(1,0) + s*cv(1,3) + vx*sig2, c11=cv(1,1) + sig2,
151
152 c20 = cv(2,0) + s*(cv(4,0) + cv(2,3)) + s*s*cv(4,3) + vx*vz*sig2,
153 c21 = cv(2,1) + s*cv(4,1) + vz*sig2,
154 c22 = cv(2,2) + 2*s*cv(4,2) + s*s*cv(4,4) + vz*vz*sig2,
155
156 c30 = cv(3,0) + s*cv(3,3), c31 = cv(3,1),
157 c32 = cv(3,2) + s*cv(3,4), c33 = cv(3,3),
158
159 c40 = cv(4,0) + s*cv(4,3), c41 = cv(4,1),
160 c42 = cv(4,2) + s*cv(4,4), c43 = cv(4,3), c44 = cv(4,4);
161
162
163 cv(0,0)=c00; cv(0,1)=c10; cv(0,2)=c20; cv(0,3)=c30; cv(0,4)=c40;
164 cv(1,0)=c10; cv(1,1)=c11; cv(1,2)=c21; cv(1,3)=c31; cv(1,4)=c41;
165 cv(2,0)=c20; cv(2,1)=c21; cv(2,2)=c22; cv(2,3)=c32; cv(2,4)=c42;
166 cv(3,0)=c30; cv(3,1)=c31; cv(3,2)=c32; cv(3,3)=c33; cv(3,4)=c43;
167 cv(4,0)=c40; cv(4,1)=c41; cv(4,2)=c42; cv(4,3)=c43; cv(4,4)=c44;
168
169 return kTRUE;
170}
171
172Bool_t AliTrackFitterKalman::Update(const AliTrackPoint *p, Double_t chi2) {
173 //
174 // Update the track params using the measured point "p"
175 //
176
177 TMatrixDSym &c=*fCov;
178 const Float_t *cov=p->GetCov();
179
180 TMatrixDSym v(3);
181 v(0,0)=cov[0]+c(0,0); v(0,1)=cov[1]+c(0,1); v(0,2)=cov[2]+c(0,2);
182 v(1,0)=cov[1]+c(1,0); v(1,1)=cov[3]+c(1,1); v(1,2)=cov[4]+c(1,2);
183 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 184 if (TMath::Abs(v.Determinant()) < 1.e-33) return kFALSE;
9c4c8863 185 v.Invert();
186
187 TMatrixD ch(5,3);
188 ch(0,0)=c(0,0); ch(0,1)=c(0,1); ch(0,2)=c(0,2);
189 ch(1,0)=c(1,0); ch(1,1)=c(1,1); ch(1,2)=c(1,2);
190 ch(2,0)=c(2,0); ch(2,1)=c(2,1); ch(2,2)=c(2,2);
191 ch(3,0)=c(3,0); ch(3,1)=c(3,1); ch(3,2)=c(3,2);
192 ch(4,0)=c(4,0); ch(4,1)=c(4,1); ch(4,2)=c(4,2);
193
194 TMatrixD k(ch,TMatrixD::kMult,v);
195
196 TMatrixD d(3,1);
197 d(0,0) = p->GetX() - fParams[0];
198 d(1,0) = p->GetY() - fParams[1];
199 d(2,0) = p->GetZ() - fParams[2];
200
201 TMatrixD x(k,TMatrixD::kMult,d);
202
203 fParams[0]+=x(0,0);
204 fParams[1]+=x(1,0);
205 fParams[2]+=x(2,0);
206 fParams[3]+=x(3,0);
207 fParams[4]+=x(4,0);
208
209 TMatrixD hc(3,5);
210 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);
211 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);
212 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);
213
214 TMatrixD s(k,TMatrixD::kMult,hc);
215
216 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);
217 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);
218 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);
219 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);
220 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);
221
222 fChi2 += chi2;
223 fNdf += 2;
224
225 return kTRUE;
226}
227
228//_____________________________________________________________________________
229Bool_t AliTrackFitterKalman::GetPCA(const AliTrackPoint &p, AliTrackPoint &i) const
230{
231 //
232 // Get the intersection point "i" between the track and the plane
233 // the point "p" belongs to.
234 //
235
236 TMatrixD t(3,1);
237 Double_t s=p.GetY() - fParams[1];
238 Double_t vx=fParams[3], vz=fParams[4];
239 t(0,0) = fParams[0] + s*vx;
240 t(1,0) = fParams[1] + s;
241 t(2,0) = fParams[2] + s*vz;
242
243 TMatrixDSym tC(3);
244 {
245 Double_t sig2=s*s/12.;
93a7cb17 246
247 tC(0,0) = vx*vx*sig2;
248 tC(1,0) = vx*sig2;
249 tC(1,1) = sig2;
250 tC(2,0) = vx*vz*sig2;
251 tC(2,1) = vz*sig2;
252 tC(2,2) = vz*vz*sig2;
9c4c8863 253
254 tC(0,1) = tC(1,0); tC(0,2) = tC(2,0);
255 tC(1,2) = tC(2,1);
256 }
257
258 TMatrixD m(3,1);
259 m(0,0)=p.GetX();
260 m(1,0)=p.GetY();
261 m(2,0)=p.GetZ();
262
263 TMatrixDSym mC(3);
264 {
265 const Float_t *cv=p.GetCov();
266 mC(0,0)=cv[0]; mC(0,1)=cv[1]; mC(0,2)=cv[2];
267 mC(1,0)=cv[1]; mC(1,1)=cv[3]; mC(1,2)=cv[4];
268 mC(2,0)=cv[2]; mC(2,1)=cv[4]; mC(2,2)=cv[5];
269 }
270
271 TMatrixDSym tmW(tC);
272 tmW+=mC;
2bece746 273 if (TMath::Abs(tmW.Determinant()) < 1.e-33) return kFALSE;
9c4c8863 274 tmW.Invert();
275
11721007 276 TMatrixD mW(tC,TMatrixD::kMult,tmW);
277 TMatrixD tW(mC,TMatrixD::kMult,tmW);
9c4c8863 278
279 TMatrixD mi(mW,TMatrixD::kMult,m);
280 TMatrixD ti(tW,TMatrixD::kMult,t);
281 ti+=mi;
282
283 TMatrixD iC(tC,TMatrixD::kMult,tmW);
284 iC*=mC;
21404e08 285 Double_t sig2=1.; // Releasing the matrix by 1 cm along the track direction
286 iC(0,0) += vx*vx*sig2;
287 iC(0,1) += vx*sig2;
288 iC(1,1) += sig2;
289 iC(0,2) += vx*vz*sig2;
290 iC(1,2) += vz*sig2;
291 iC(2,2) += vz*vz*sig2;
292
9c4c8863 293
294 Float_t cov[6]={
295 iC(0,0), iC(0,1), iC(0,2),
296 iC(1,1), iC(1,2),
297 iC(2,2)
298 };
299 i.SetXYZ(ti(0,0),ti(1,0),ti(2,0),cov);
300 UShort_t id=p.GetVolumeID();
301 i.SetVolumeID(id);
302
303 return kTRUE;
304}
305
306
307//_____________________________________________________________________________
308void
309AliTrackFitterKalman::SetSeed(const Double_t par[5], const Double_t cov[15]) {
310 //
311 // Set the initial approximation for the track parameters
312 //
313 for (Int_t i=0; i<5; i++) fParams[i]=par[i];
314 fParams[5]=0.;
315
316 delete fCov;
317 fCov=new TMatrixDSym(5);
318 TMatrixDSym &cv=*fCov;
319
320 cv(0,0)=cov[0 ];
321 cv(1,0)=cov[1 ]; cv(1,1)=cov[2 ];
322 cv(2,0)=cov[3 ]; cv(2,1)=cov[4 ]; cv(2,2)=cov[5 ];
323 cv(3,0)=cov[6 ]; cv(3,1)=cov[7 ]; cv(3,2)=cov[8 ]; cv(3,3)=cov[9 ];
324 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];
325
326 cv(0,1)=cv(1,0);
327 cv(0,2)=cv(2,0); cv(1,2)=cv(2,1);
328 cv(0,3)=cv(3,0); cv(1,3)=cv(3,1); cv(2,3)=cv(3,2);
329 cv(0,4)=cv(4,0); cv(1,4)=cv(4,1); cv(2,4)=cv(4,2); cv(3,4)=cv(4,3);
330
57c8a1c2 331 fSeed=kTRUE;
9c4c8863 332}