]>
Commit | Line | Data |
---|---|---|
8296a418 | 1 | |
2 | /* | |
3 | This test macro to test AliTracker functionality. | |
4 | Additional focus on the tracking of cosmic tracks . | |
5 | ||
6 | Test: | |
7 | 1. Simulate random tracks - AliTracker used to propagate track | |
8 | 2. Missalign and smear space points | |
9 | 3. Fit the tracks | |
10 | ||
11 | 4. Visualize results/residual/pulls using tree draw | |
12 | ||
13 | The input MC paramters are stored together with reconstructed output in the tree. | |
14 | ||
15 | ||
16 | Usage: | |
17 | .L AliTrackerTest.C+g | |
4e846b22 | 18 | AliTrackerTest(1000) |
8296a418 | 19 | TFile f("fit.root"); |
20 | ||
21 | Make a simple plots: | |
22 | //Pulls in P3 parameter - tangent lambda | |
23 | fit->Draw("(seedF0.fP[3]-mcU.fP[3])/sqrt(seedF0.fC[9])>>hisSP3(100,-5,5)","") | |
24 | //Pulls of seeding for seed | |
25 | fit->Draw("(seedF0.fP[4]-mcU.fP[4])/sqrt(seedF0.fC[14])>>hisSP4(100,-5,5)","") | |
26 | // | |
27 | //Pulls in P3 parameter - tangent lambda | |
28 | fit->Draw("(trackF0.fP[3]-mcU.fP[3])/sqrt(trackF02.fC[9])>>hisP3(100,-5,5)","") | |
29 | //Pulls in P4 parameter | |
30 | fit->Draw("(trackF0.fP[4]-mcU.fP[4])/sqrt(trackF02.fC[14])>>hisP4(100,-5,5)","") | |
31 | ||
32 | // | |
33 | // | |
34 | // seeding procedure test (errors scaled 0.01 cm) | |
35 | fit->Draw("(seedF0.Pt()-mcD.Pt())/mcD.Pt()^2:mcD.Pt()>>hisSPt(10,0,400,100,-0.005,0.005)","abs(seedF0.fP[4])<3&&abs(pointsU.fNPoints+pointsD.fNPoints)>300","colz"); | |
36 | //"standart track | |
37 | fit->Draw("(trackF0.Pt()-mcU.Pt())/mcU.Pt()^2:mcU.Pt()>>his0Pt(10,0,400,100,-0.005,0.005)","abs(trackF0.fP[4])<3&&abs(pointsU.fNPoints+pointsD.fNPoints)>300","colz"); | |
38 | // misaligned track - up down | |
39 | fit->Draw("(trackF02.Pt()-mcU.Pt())/mcU.Pt()^2:mcU.Pt()>>his2Pt(10,0,400,100,-0.005,0.005)","abs(trackF02.fP[4])<3&&abs(pointsU.fNPoints+pointsD.fNPoints)>300","colz"); | |
40 | hisSPt->FitSlicesY(); | |
41 | his0Pt->FitSlicesY(); | |
42 | his2Pt->FitSlicesY(); | |
43 | ||
44 | */ | |
45 | ||
46 | //ROOT includes | |
47 | #include <iostream> | |
48 | #include "TRandom.h" | |
49 | #include "TRandom3.h" | |
50 | #include "TNtuple.h" | |
51 | #include "TStopwatch.h" | |
52 | #include "TDatabasePDG.h" | |
53 | #include "TMath.h" | |
54 | #include "TGeoManager.h" | |
55 | #include "TClonesArray.h" | |
56 | #include "TTree.h" | |
57 | #include "TFile.h" | |
58 | //AliRoot includes | |
59 | #include "AliGeomManager.h" | |
60 | #include "AliMagF.h" | |
61 | #include "AliESDVertex.h" | |
62 | #include "AliExternalTrackParam.h" | |
63 | #include "TTreeStream.h" | |
64 | #include "AliTrackPointArray.h" | |
65 | #include "AliTrackerBase.h" | |
66 | #include "AliTracker.h" | |
4e846b22 | 67 | #include "TMatrixD.h" |
8296a418 | 68 | |
69 | // | |
70 | // | |
4e846b22 | 71 | //#include "refitTrack.C" |
8296a418 | 72 | |
73 | void TestRotateMI(AliExternalTrackParam *paramIn, TTreeSRedirector *pcstream); | |
4e846b22 | 74 | void AliTrackerBaseUpdateTrack(AliExternalTrackParam &track1, const AliExternalTrackParam &track2); |
75 | void TestUpdate(AliExternalTrackParam ¶m0, const AliExternalTrackParam ¶m1); | |
8296a418 | 76 | |
77 | ||
78 | Int_t simulateCosmicTrack(AliExternalTrackParam ¶mU, AliExternalTrackParam ¶mD, | |
79 | AliTrackPointArray &pointsU, AliTrackPointArray &pointsD, | |
80 | AliTrackPointArray &pointsF){ | |
81 | // | |
82 | // Toy - Simulate cosmic muon track | |
83 | // space points genearted with the step 1 cm - at given radius | |
84 | // | |
85 | // Return value - number of points | |
86 | // paramU - parameters at the beginning of track | |
87 | // paramD - parameters at the end of track | |
88 | // pointsU - points in upper half of | |
89 | // pointsD - points in lower part | |
90 | // pointsF - all points along trajectory | |
91 | // | |
92 | Double_t rTPC1=250; | |
93 | Double_t rTPC0=80; | |
94 | Double_t kMuon = TDatabasePDG::Instance()->GetParticle("mu+")->Mass(); | |
95 | Double_t kMaxSnp = 0.85; | |
96 | Double_t xyz[3]={0,0,0}; | |
97 | Double_t xyzdw[3]={0,0,0}; | |
98 | Double_t pxyzup[3]={0,0,0}; | |
99 | Double_t pxyzdw[3]={0,0,0}; | |
100 | Double_t pxyz[3]={0,0,0}; | |
101 | Double_t cv[21]; | |
102 | for (Int_t i=0; i<21; i++) cv[i]=0; | |
103 | Float_t covPoint[6]={0,0,0,0,0,0}; | |
104 | UShort_t volid=0; | |
105 | // | |
106 | static TClonesArray arrup("AliTrackPoint",500); | |
107 | static TClonesArray arrdw("AliTrackPoint",500); | |
108 | static TClonesArray arr("AliTrackPoint",500); | |
109 | if (arrup[0]==0){ | |
110 | // init point arrays | |
111 | for (Int_t i=0;i<500;i++){ | |
112 | new (arrup[i]) AliTrackPoint; | |
113 | new (arrdw[i]) AliTrackPoint; | |
114 | new (arr[i]) AliTrackPoint; | |
115 | } | |
116 | } | |
117 | ||
118 | // | |
119 | xyz[0]=(gRandom->Rndm()-0.5)*250; | |
120 | xyz[1]=250; | |
121 | xyz[2]=(gRandom->Rndm()-0.5)*100; | |
122 | // | |
123 | Double_t pt = gRandom->Exp(8.); | |
124 | if (gRandom->Rndm()>0.3) pt= gRandom->Exp(50.); | |
125 | if (gRandom->Rndm()>0.6) pt= gRandom->Rndm()*400; | |
126 | // Double_t pt = gRandom->Exp(60.); | |
127 | Double_t pz = gRandom->Gaus(0,0.3)*pt; | |
128 | Double_t phi = gRandom->Gaus(0,0.3); | |
129 | pxyz[0] = pt*TMath::Sin(phi); | |
130 | pxyz[1] = pt*TMath::Cos(phi); | |
131 | pxyz[2] = pz; | |
132 | Short_t sign= (gRandom->Rndm()>0.5)? -1:1; | |
133 | // | |
134 | pxyzup[0]=pxyz[0]; | |
135 | pxyzup[1]=pxyz[1]; | |
136 | pxyzup[2]=pxyz[2]; | |
137 | // | |
138 | AliExternalTrackParam lparamU(xyz, pxyzup,cv,sign); | |
139 | Double_t alpha = TMath::ATan2(xyz[1],xyz[0]); | |
140 | lparamU.Rotate(alpha); | |
141 | paramU=lparamU; | |
142 | // | |
143 | // | |
144 | // | |
145 | Double_t txyzup[3]={0,0,0}; | |
146 | Double_t txyzdw[3]={0,0,0}; | |
147 | // | |
148 | Int_t npointsUp=0; | |
149 | AliTrackerBase::PropagateTrackTo(&lparamU,rTPC1,kMuon,3,kTRUE,kMaxSnp); | |
150 | paramU=lparamU; | |
151 | for (Double_t r=rTPC1; r>0; r-=1){ | |
4e846b22 | 152 | Bool_t status = AliTrackerBase::PropagateTrackToBxByBz(&lparamU,r,kMuon,3,kTRUE,kMaxSnp); |
8296a418 | 153 | if (!status) break; |
154 | if (TMath::Abs(lparamU.GetSnp())>kMaxSnp) break; | |
155 | if (r<rTPC0) continue; | |
156 | lparamU.GetXYZ(txyzup); | |
157 | new (arrup[npointsUp]) AliTrackPoint(txyzup[0],txyzup[1],txyzup[2],covPoint,volid,0.,0.,0.,0); | |
158 | npointsUp++; | |
159 | } | |
160 | // | |
161 | lparamU.GetXYZ(txyzup); | |
162 | Double_t bz=AliTrackerBase::GetBz(txyzup); | |
163 | Double_t maxd=100000; | |
164 | Double_t pvtx[3]={0,0,0}; | |
165 | Double_t sigmavtx[3]={0.01,0.01,3000}; | |
166 | AliESDVertex *vtx= new AliESDVertex(pvtx,sigmavtx,"vertex"); | |
167 | Double_t dz[2]={0,0}; | |
168 | Double_t cvtx[3]={0.0,0.0,0}; | |
169 | lparamU.PropagateToDCA(vtx,bz,maxd,dz,cvtx); | |
170 | // | |
171 | // make step to other side | |
172 | lparamU.PropagateTo(-30,bz); | |
173 | lparamU.GetXYZ(xyzdw); | |
174 | lparamU.GetPxPyPz(pxyzdw); | |
175 | // invert the sign of the momentum | |
176 | pxyzdw[0]=-pxyzdw[0]; | |
177 | pxyzdw[1]=-pxyzdw[1]; | |
178 | pxyzdw[2]=-pxyzdw[2]; | |
179 | Short_t sign2=-sign; | |
180 | AliExternalTrackParam lparamD(xyzdw,pxyzdw,cv,sign2); | |
181 | lparamU.GetXYZ(xyzdw); | |
182 | lparamU.GetPxPyPz(pxyzdw); | |
183 | Double_t alphadw = TMath::ATan2(xyzdw[1],xyzdw[0]); | |
184 | lparamD.Rotate(alphadw);//I have to rotate gobal to local coordenate externalparam | |
185 | Double_t radius0=TMath::Sqrt(xyzdw[1]*xyzdw[1]+xyzdw[0]*xyzdw[0]); | |
186 | Int_t npointsDown=0; | |
187 | for (Double_t r=radius0; r<rTPC1; r+=1){ | |
4e846b22 | 188 | Bool_t status = AliTrackerBase::PropagateTrackToBxByBz(&lparamD,r,kMuon,3,kTRUE,0.99); |
8296a418 | 189 | if (!status) continue; |
190 | if (TMath::Abs(lparamD.GetSnp())>kMaxSnp) continue; | |
191 | if(r>rTPC0){ | |
192 | lparamD.GetXYZ(txyzdw); | |
193 | new (arrdw[npointsDown]) AliTrackPoint(txyzdw[0],txyzdw[1],txyzdw[2],covPoint,volid,0.,0.,0.,0); | |
194 | npointsDown++; | |
195 | } | |
196 | } | |
197 | // | |
198 | // Fill MC point arrays | |
199 | // | |
200 | Int_t npoints=npointsUp+npointsDown; | |
201 | AliTrackPointArray lpointsF(npoints); | |
202 | AliTrackPointArray lpointsU(npointsUp); | |
203 | AliTrackPointArray lpointsD(npointsDown); | |
204 | for (Int_t i=0; i<npointsUp;i++){ | |
205 | AliTrackPoint *point = (AliTrackPoint*)arrup[i]; | |
206 | lpointsF.AddPoint(i,point); | |
207 | lpointsU.AddPoint(i,point); | |
208 | } | |
209 | // | |
210 | for (Int_t i=0; i<npointsDown;i++){ | |
211 | AliTrackPoint *point = (AliTrackPoint*)arrdw[i]; | |
212 | lpointsF.AddPoint(i+npointsUp,point); | |
213 | lpointsD.AddPoint(i,point); | |
214 | } | |
215 | // export points | |
4e846b22 | 216 | AliTrackerBase::PropagateTrackToBxByBz(&lparamD,rTPC1,kMuon,3,kTRUE,kMaxSnp); |
8296a418 | 217 | paramD=lparamD; |
218 | // | |
219 | pointsU=lpointsU; | |
220 | pointsD=lpointsD; | |
221 | pointsF=lpointsF; | |
222 | return npoints; | |
223 | } | |
224 | ||
225 | ||
226 | AliTrackPointArray * SmearPoints(AliTrackPointArray &pointArray, Double_t sigmaY, Double_t sigmaZ, Double_t shiftY,Double_t shiftZ){ | |
227 | // | |
228 | // Smear ideal points form the simulation | |
229 | // 1. Smear the input points (in the "local frame") | |
230 | // 2. Assing corresponding covariance matrix | |
231 | // 3. Add systematic shift - shift y and shift z | |
232 | ||
233 | Int_t npoints=pointArray.GetNPoints(); | |
234 | AliTrackPointArray *outputArray = new AliTrackPointArray(npoints); | |
235 | Double_t xyz[3]={0,0,0}; | |
236 | Float_t covPoint[6]={0,0,0, sigmaY*sigmaY,0,sigmaZ*sigmaZ}; //covaraince at the local frame | |
237 | // | |
238 | // | |
239 | for (Int_t ipoint=0; ipoint<npoints; ipoint++){ | |
240 | AliTrackPoint pointIn; | |
241 | pointArray.GetPoint(pointIn,ipoint); | |
242 | Double_t alpha = TMath::ATan2(pointIn.GetY(),pointIn.GetX()); | |
243 | AliTrackPoint pr = pointIn.Rotate(alpha); | |
244 | xyz[0]=pr.GetX(); //local x | |
245 | xyz[1]=pr.GetY()+gRandom->Gaus(0,sigmaY); //local y | |
246 | xyz[2]=pr.GetZ()+gRandom->Gaus(0,sigmaZ); //local z | |
247 | if (pointIn.GetY()>0) xyz[1]+=shiftY; | |
248 | if (pointIn.GetY()>0) xyz[2]+=shiftZ; | |
249 | // | |
250 | pr.SetXYZ(xyz[0],xyz[1],xyz[2],covPoint); // set covariance matrix | |
251 | AliTrackPoint pg= pr.Rotate(-alpha); | |
252 | AliTrackPoint prCheck= pg.Rotate(alpha); | |
253 | outputArray->AddPoint(ipoint,&pg); | |
254 | } | |
255 | return outputArray; | |
256 | } | |
257 | ||
258 | ||
259 | ||
260 | AliExternalTrackParam * MakeSeed(AliTrackPointArray &pointArray, Int_t seedDelta){ | |
261 | // | |
262 | // Example: creation of seed | |
263 | // Make seed for array of track points | |
264 | // seedDelta - gap between seeding points | |
265 | // | |
266 | Int_t npoints=pointArray.GetNPoints(); | |
267 | if(npoints<=3) return 0; //not enough points to make a trac | |
268 | if (npoints-2*seedDelta-1<0) return 0; | |
269 | AliTrackPoint point1; | |
270 | AliTrackPoint point2; | |
271 | AliTrackPoint point3; | |
272 | pointArray.GetPoint(point1,npoints-1); | |
273 | pointArray.GetPoint(point2,npoints-seedDelta-1); | |
274 | pointArray.GetPoint(point3,npoints-2*seedDelta-1); | |
275 | // | |
276 | AliExternalTrackParam * trackParam = AliTrackerBase::MakeSeed(point1, point2, point3); | |
277 | return trackParam; | |
278 | } | |
279 | ||
280 | ||
281 | void AliTrackerTest(Int_t ntracks) { | |
282 | // | |
283 | // | |
284 | // | |
285 | TGeoManager::Import("./geometry.root"); | |
286 | AliGeomManager::LoadGeometry("./geometry.root"); | |
287 | AliMagF::BMap_t smag = AliMagF::k5kG; | |
288 | Double_t kMuon = TDatabasePDG::Instance()->GetParticle("mu+")->Mass(); | |
289 | TGeoGlobalMagField::Instance()->SetField(new AliMagF("Maps","Maps", -1., -1., smag)); | |
290 | // | |
291 | ||
292 | TTreeSRedirector *pcstream = new TTreeSRedirector("fit.root"); | |
293 | AliExternalTrackParam paramU; | |
294 | AliExternalTrackParam paramD; | |
295 | AliTrackPointArray pointsU; | |
296 | AliTrackPointArray pointsD; | |
297 | AliTrackPointArray pointsF; | |
298 | ||
299 | for (Int_t i=0;i<ntracks;i++) { | |
300 | if (i%10==0) printf("Track\t%d\n",i); | |
301 | Int_t npoints = simulateCosmicTrack(paramU, paramD, pointsU, pointsD, pointsF); | |
302 | if (npoints<10) continue; | |
303 | // | |
304 | // point for test of seeding - with scaled errors | |
305 | AliTrackPointArray* pointsFS=SmearPoints(pointsF, 0.01,0.01,0,0); | |
306 | AliTrackPointArray* pointsDS=SmearPoints(pointsD, 0.01,0.01,0,0); | |
307 | AliTrackPointArray* pointsUS=SmearPoints(pointsU, 0.01,0.01,0,0); | |
308 | //test of seeding routines | |
309 | AliExternalTrackParam *seedF0 = MakeSeed(*pointsFS,50); | |
310 | AliExternalTrackParam *seedD0 = MakeSeed(*pointsDS,50); | |
311 | AliExternalTrackParam *seedU0 = MakeSeed(*pointsUS,50); | |
312 | // points smeeared according TPC resolution | |
313 | AliTrackPointArray* pointsF0=SmearPoints(pointsF, 0.1,0.1,0,0); | |
314 | AliTrackPointArray* pointsD0=SmearPoints(pointsD, 0.1,0.1,0,0); | |
315 | AliTrackPointArray* pointsU0=SmearPoints(pointsU, 0.1,0.1,0,0); | |
316 | // points smeared according TPCresolution - + systematic shift | |
317 | AliTrackPointArray* pointsF02=SmearPoints(pointsF, 0.1,0.1,0.2,0.2); | |
318 | AliTrackPointArray* pointsD02=SmearPoints(pointsD, 0.1,0.1,0.2,0.2); | |
319 | AliTrackPointArray* pointsU02=SmearPoints(pointsU, 0.1,0.1,0.2,0.2); | |
320 | //test of tracking | |
321 | AliExternalTrackParam *trackF0 = MakeSeed(*pointsF0,10); | |
322 | AliExternalTrackParam *trackD0 = MakeSeed(*pointsD0,10); | |
323 | AliExternalTrackParam *trackU0 = MakeSeed(*pointsU0,10); | |
324 | AliExternalTrackParam *trackF02 = MakeSeed(*pointsF02,10); | |
325 | AliExternalTrackParam *trackD02 = MakeSeed(*pointsD02,10); | |
326 | AliExternalTrackParam *trackU02 = MakeSeed(*pointsU02,10); | |
327 | // | |
328 | // | |
329 | if (!trackF0) continue; | |
330 | if (!trackD0) continue; | |
331 | if (!trackU0) continue; | |
332 | if (!seedF0) continue; | |
333 | if (!seedD0) continue; | |
334 | if (!seedU0) continue; | |
335 | AliTrackerBase::FitTrack(trackF0, pointsF0,kMuon,3); | |
336 | AliTrackerBase::FitTrack(trackU0, pointsU0,kMuon,3); | |
337 | AliTrackerBase::FitTrack(trackD0, pointsD0,kMuon,3); | |
338 | AliTrackerBase::FitTrack(trackF02, pointsF02,kMuon,3); | |
339 | AliTrackerBase::FitTrack(trackD02, pointsD02,kMuon,3); | |
4e846b22 | 340 | AliTrackerBase::FitTrack(trackU02, pointsU02,kMuon,3); |
341 | // | |
342 | // test update of 2 track params | |
343 | AliExternalTrackParam utrackU0(*trackU0); | |
344 | TestUpdate(utrackU0,*trackD0); | |
345 | // | |
346 | ||
8296a418 | 347 | // |
348 | TestRotateMI(trackF0, pcstream); | |
349 | if (trackF0&&trackD0&&trackU0){ | |
350 | (*pcstream)<<"fit"<< | |
351 | "pointsD.="<<&pointsD<< | |
352 | "pointsD0.="<<pointsD0<< | |
353 | "pointsD02.="<<pointsD02<< | |
354 | // | |
355 | "pointsU.="<<&pointsU<< | |
356 | "pointsU0.="<<pointsU0<< | |
357 | "pointsU02.="<<pointsU02<< | |
358 | // | |
359 | "pointsF.="<<&pointsF<< | |
360 | "pointsF0.="<<pointsF0<< | |
361 | "pointsF02.="<<pointsF02<< | |
362 | // | |
363 | "mcU.="<<¶mU<< //MC track down | |
364 | "mcD.="<<¶mD<< //MC track top | |
365 | // track fit - 0 misalignemnt | |
366 | "seedF0.="<<seedF0<< //full track | |
367 | "seedD0.="<<seedD0<< //down track | |
368 | "seedU0.="<<seedU0<< //up track | |
369 | // | |
370 | "trackF0.="<<trackF0<< //full track | |
371 | "trackD0.="<<trackD0<< //down track | |
372 | "trackU0.="<<trackU0<< //up track | |
4e846b22 | 373 | "utrackU0.="<<&utrackU0<< //up track - updated |
8296a418 | 374 | // track fit - 0.2 cm misalignemnt |
375 | "trackF02.="<<trackF02<< //full track | |
376 | "trackD02.="<<trackD02<< //down track | |
377 | "trackU02.="<<trackU02<< //up track | |
378 | "\n"; | |
379 | } | |
380 | } | |
381 | delete pcstream; | |
382 | ||
383 | } | |
384 | ||
385 | void TestRotateMI(AliExternalTrackParam *paramIn, TTreeSRedirector *pcstream){ | |
386 | // | |
387 | // test of rotation function | |
388 | // rotate by 360 degrees | |
389 | // dump the state vector to the tree after each rotation | |
390 | AliExternalTrackParam param(*paramIn); | |
391 | AliExternalTrackParam paramMI(*paramIn); | |
392 | for (Int_t idiv=0; idiv<=18; idiv++){ | |
393 | Double_t alphaRot = paramIn->GetAlpha()+2*TMath::Pi()*idiv/18.; | |
394 | param.Rotate(alphaRot); | |
395 | paramMI.RotateMI(alphaRot); | |
396 | (*pcstream)<<"rotateTest"<< | |
397 | "pIn.="<<paramIn<< | |
398 | "pRot.="<<¶m<< | |
399 | "pRotMI.="<<¶mMI<< | |
400 | "idiv="<<idiv<< | |
401 | "\n"; | |
402 | } | |
403 | /* | |
404 | to check: | |
405 | ||
406 | */ | |
407 | } | |
4e846b22 | 408 | |
409 | ||
410 | void TestUpdate(AliExternalTrackParam ¶m0, const AliExternalTrackParam ¶m1){ | |
411 | // | |
412 | // Test function of update | |
413 | // | |
414 | // fit->Draw("(utrackU0.fP[4]-mcU.fP[4])/sqrt(utrackU0.fC[14])","abs(utrackU0.fP[0]-trackU0.fP[0])<1") | |
415 | // fit->Draw("(trackU0.fP[4]-mcU.fP[4])/sqrt(utrackU0.fC[14])","abs(utrackU0.fP[0]-trackU0.fP[0])<1") | |
416 | ||
417 | ||
418 | // | |
419 | Double_t kMuon = TDatabasePDG::Instance()->GetParticle("mu+")->Mass(); | |
420 | Double_t kMaxSnp = 0.85; | |
421 | // | |
422 | AliExternalTrackParam track1(param1); // make a local copy of param1 | |
423 | track1.Rotate(param0.GetAlpha()); | |
424 | AliTrackerBase::PropagateTrackToBxByBz(&track1,param0.GetX(),kMuon,3,kFALSE,kMaxSnp); | |
425 | AliTrackerBaseUpdateTrack(param0,track1); | |
426 | } | |
427 | ||
428 | ||
429 | ||
430 | ||
431 | void AliTrackerBaseUpdateTrack(AliExternalTrackParam &track1, const AliExternalTrackParam &track2){ | |
432 | // | |
433 | // Update track 1 with track 2 | |
434 | // | |
435 | // | |
436 | // | |
437 | TMatrixD vecXk(5,1); // X vector | |
438 | TMatrixD covXk(5,5); // X covariance | |
439 | TMatrixD matHk(5,5); // vector to mesurement | |
440 | TMatrixD measR(5,5); // measurement error | |
441 | TMatrixD vecZk(5,1); // measurement | |
442 | // | |
443 | TMatrixD vecYk(5,1); // Innovation or measurement residual | |
444 | TMatrixD matHkT(5,5); | |
445 | TMatrixD matSk(5,5); // Innovation (or residual) covariance | |
446 | TMatrixD matKk(5,5); // Optimal Kalman gain | |
447 | TMatrixD mat1(5,5); // update covariance matrix | |
448 | TMatrixD covXk2(5,5); // | |
449 | TMatrixD covOut(5,5); | |
450 | // | |
451 | Double_t *param1=(Double_t*) track1.GetParameter(); | |
452 | Double_t *covar1=(Double_t*) track1.GetCovariance(); | |
453 | Double_t *param2=(Double_t*) track2.GetParameter(); | |
454 | Double_t *covar2=(Double_t*) track2.GetCovariance(); | |
455 | // | |
456 | // copy data to the matrix | |
457 | for (Int_t ipar=0; ipar<5; ipar++){ | |
458 | for (Int_t jpar=0; jpar<5; jpar++){ | |
459 | covXk(ipar,jpar) = covar1[track1.GetIndex(ipar, jpar)]; | |
460 | measR(ipar,jpar) = covar2[track2.GetIndex(ipar, jpar)]; | |
461 | matHk(ipar,jpar)=0; | |
462 | mat1(ipar,jpar)=0; | |
463 | } | |
464 | vecXk(ipar,0) = param1[ipar]; | |
465 | vecZk(ipar,0) = param2[ipar]; | |
466 | matHk(ipar,ipar)=1; | |
467 | mat1(ipar,ipar)=1; | |
468 | } | |
469 | // | |
470 | // | |
471 | // | |
472 | // | |
473 | // | |
474 | vecYk = vecZk-matHk*vecXk; // Innovation or measurement residual | |
475 | matHkT=matHk.T(); matHk.T(); | |
476 | matSk = (matHk*(covXk*matHkT))+measR; // Innovation (or residual) covariance | |
477 | matSk.Invert(); | |
478 | matKk = (covXk*matHkT)*matSk; // Optimal Kalman gain | |
479 | vecXk += matKk*vecYk; // updated vector | |
480 | covXk2 = (mat1-(matKk*matHk)); | |
481 | covOut = covXk2*covXk; | |
482 | // | |
483 | // | |
484 | // | |
485 | // copy from matrix to parameters | |
486 | if (0) { | |
487 | vecXk.Print(); | |
488 | vecZk.Print(); | |
489 | // | |
490 | measR.Print(); | |
491 | covXk.Print(); | |
492 | covOut.Print(); | |
493 | // | |
494 | track1.Print(); | |
495 | track2.Print(); | |
496 | } | |
497 | ||
498 | for (Int_t ipar=0; ipar<5; ipar++){ | |
499 | param1[ipar]= vecXk(ipar,0) ; | |
500 | for (Int_t jpar=0; jpar<5; jpar++){ | |
501 | covar1[track1.GetIndex(ipar, jpar)]=covOut(ipar,jpar); | |
502 | } | |
503 | } | |
504 | } | |
505 | ||
506 |