BmnRoot
Loading...
Searching...
No Matches
L1Filtration.h
Go to the documentation of this file.
1#ifndef L1Filtration_h
2#define L1Filtration_h
3
4#include "CbmL1Def.h"
5#include "L1UMeasurementInfo.h"
6#include "L1XYMeasurementInfo.h"
7#include "L1TrackPar.h"
8
9//#define cnst static const fvec
10#define cnst const fvec
11
12//inline void L1Filter( L1TrackPar &T, L1UMeasurementInfo &info, fvec u, fvec w = 1.)
13inline void L1Filter( L1TrackPar &T, L1UMeasurementInfo &info, fvec u, fvec w = 1., fvec *du2 = 0) //AZ
14{
15 fvec wi, zeta, zetawi, HCH;
16 fvec F0, F1, F2, F3, F4;
17 fvec K1, K2, K3, K4;
18
19 zeta = info.cos_phi*T.x + info.sin_phi*T.y - u;
20
21 // F = CH'
22 F0 = info.cos_phi*T.C00 + info.sin_phi*T.C10;
23 F1 = info.cos_phi*T.C10 + info.sin_phi*T.C11;
24
25 HCH = ( F0*info.cos_phi + F1*info.sin_phi );
26
27 F2 = info.cos_phi*T.C20 + info.sin_phi*T.C21;
28 F3 = info.cos_phi*T.C30 + info.sin_phi*T.C31;
29 F4 = info.cos_phi*T.C40 + info.sin_phi*T.C41;
30
31#if 0 // use mask
32 const fvec mask = (HCH < info.sigma2 * 16.);
33 wi = w/( (mask & info.sigma2) +HCH );
34 zetawi = zeta *wi;
35 T.chi2 += mask & (zeta * zetawi);
36#else
37 if (du2) wi = w / (*du2 + HCH); //AZ
38 else //AZ
39 wi = w/( info.sigma2 + HCH );
40 //cout << wi << " " << *du2 << " " << info.sigma2 << endl; //AZ
41 zetawi = zeta *wi;
42 T.chi2 += zeta * zetawi;
43#endif // 0
44 T.NDF += w;
45
46 K1 = F1*wi;
47 K2 = F2*wi;
48 K3 = F3*wi;
49 K4 = F4*wi;
50
51 T.x -= F0*zetawi;
52 T.y -= F1*zetawi;
53 T.tx -= F2*zetawi;
54 T.ty -= F3*zetawi;
55 T.qp -= F4*zetawi;
56
57 T.C00-= F0*F0*wi;
58 T.C10-= K1*F0;
59 T.C11-= K1*F1;
60 T.C20-= K2*F0;
61 T.C21-= K2*F1;
62 T.C22-= K2*F2;
63 T.C30-= K3*F0;
64 T.C31-= K3*F1;
65 T.C32-= K3*F2;
66 T.C33-= K3*F3;
67 T.C40-= K4*F0;
68 T.C41-= K4*F1;
69 T.C42-= K4*F2;
70 T.C43-= K4*F3;
71 T.C44-= K4*F4;
72}
73
74inline void L1FilterChi2( const L1UMeasurementInfo &info, const fvec& x, const fvec& y, const fvec& C00,
75 //const fvec& C10, const fvec& C11, fvec& chi2, const fvec& u )
76 const fvec& C10, const fvec& C11, fvec& chi2, const fvec& u, const fvec* du2 = 0 ) //AZ
77{
78 fvec zeta, HCH;
79 fvec F0, F1;
80
81 zeta = info.cos_phi*x + info.sin_phi*y - u;
82
83 // F = CH'
84 F0 = info.cos_phi*C00 + info.sin_phi*C10;
85 F1 = info.cos_phi*C10 + info.sin_phi*C11;
86
87 HCH = ( F0*info.cos_phi + F1*info.sin_phi );
88
89 if (du2) chi2 += zeta * zeta / (*du2 + HCH); //AZ
90 else //AZ
91 chi2 += zeta * zeta / (info.sigma2 + HCH) ;
92}
93
94inline void L1FilterChi2XYC00C10C11( const L1UMeasurementInfo &info, fvec& x, fvec& y, fvec& C00, fvec& C10,
95 //fvec& C11, fvec& chi2, const fvec& u )
96 fvec& C11, fvec& chi2, const fvec& u, const fvec* du2 = 0 ) //AZ
97{
98 fvec wi, zeta, zetawi, HCH;
99 fvec F0, F1;
100 fvec K1;
101
102 zeta = info.cos_phi*x + info.sin_phi*y - u;
103
104 // F = CH'
105 F0 = info.cos_phi*C00 + info.sin_phi*C10;
106 F1 = info.cos_phi*C10 + info.sin_phi*C11;
107
108 HCH = ( F0*info.cos_phi + F1*info.sin_phi );
109
110 if (du2) wi = 1./(*du2 + HCH); //AZ
111 else //AZ
112 wi = 1./(info.sigma2 + HCH);
113 zetawi = zeta *wi;
114 chi2 += zeta * zetawi ;
115
116 K1 = F1*wi;
117
118 x -= F0*zetawi;
119 y -= F1*zetawi;
120
121 C00-= F0*F0*wi;
122 C10-= K1*F0;
123 C11-= K1*F1;
124}
125
126inline void L1FilterVtx( L1TrackPar &T, fvec x, fvec y, L1XYMeasurementInfo &info,
127 fvec extrDx, fvec extrDy, fvec J[] )
128{
129 cnst TWO = 2.;
130
131 fvec zeta0, zeta1, S00, S10, S11, si;
132 fvec F00, F10, F20, F30, F40, F01, F11, F21, F31, F41 ;
133 fvec K00, K10, K20, K30, K40, K01, K11, K21, K31, K41;
134
135 //zeta0 = T.x + J[0]*T.tx + J[1]*T.ty + J[2]*T.qp - x;
136 //zeta1 = T.y + J[3]*T.tx + J[4]*T.ty + J[5]*T.qp - y;
137
138 zeta0 = T.x + extrDx - x;
139 zeta1 = T.y + extrDy - y;
140
141 // H = 1 0 J[0] J[1] J[2]
142 // 0 1 J[3] J[4] J[5]
143
144 // F = CH'
145 F00 = T.C00; F01 = T.C10;
146 F10 = T.C10; F11 = T.C11;
147 F20 = J[0]*T.C22; F21 = J[3]*T.C22;
148 F30 = J[1]*T.C33; F31 = J[4]*T.C33;
149 F40 = J[2]*T.C44; F41 = J[5]*T.C44;
150
151 S00 = info.C00 + F00 + J[0]*F20 + J[1]*F30 + J[2]*F40;
152 S10 = info.C10 + F10 + J[3]*F20 + J[4]*F30 + J[5]*F40;
153 S11 = info.C11 + F11 + J[3]*F21 + J[4]*F31 + J[5]*F41;
154
155 si = 1./(S00*S11 - S10*S10);
156 //si = fvec(rcp(fvec((S00*S11 - S10*S10)[0])));
157 fvec S00tmp = S00;
158 S00 = si*S11;
159 S10 = -si*S10;
160 S11 = si*S00tmp;
161
162 T.chi2+= zeta0*zeta0*S00 + 2.*zeta0*zeta1*S10 + zeta1*zeta1*S11;
163 T.NDF += TWO;
164
165 K00 = F00*S00 + F01*S10; K01 = F00*S10 + F01*S11;
166 K10 = F10*S00 + F11*S10; K11 = F10*S10 + F11*S11;
167 K20 = F20*S00 + F21*S10; K21 = F20*S10 + F21*S11;
168 K30 = F30*S00 + F31*S10; K31 = F30*S10 + F31*S11;
169 K40 = F40*S00 + F41*S10; K41 = F40*S10 + F41*S11;
170
171 T.x -= K00*zeta0 + K01*zeta1;
172 T.y -= K10*zeta0 + K11*zeta1;
173 T.tx -= K20*zeta0 + K21*zeta1;
174 T.ty -= K30*zeta0 + K31*zeta1;
175 T.qp -= K40*zeta0 + K41*zeta1;
176
177 T.C00-= ( K00*F00 + K01*F01 );
178 T.C10-= ( K10*F00 + K11*F01 );
179 T.C11-= ( K10*F10 + K11*F11 );
180 T.C20 = -( K20*F00 + K21*F01 );
181 T.C21 = -( K20*F10 + K21*F11 );
182 T.C22-= ( K20*F20 + K21*F21 );
183 T.C30 = -( K30*F00 + K31*F01 );
184 T.C31 = -( K30*F10 + K31*F11 );
185 T.C32 = -( K30*F20 + K31*F21 );
186 T.C33-= ( K30*F30 + K31*F31 );
187 T.C40 = -( K40*F00 + K41*F01 );
188 T.C41 = -( K40*F10 + K41*F11 );
189 T.C42 = -( K40*F20 + K41*F21 );
190 T.C43 = -( K40*F30 + K41*F31 );
191 T.C44-= ( K40*F40 + K41*F41 );
192}
193
194
195inline void L1FilterXY( L1TrackPar &T, fvec x, fvec y, L1XYMeasurementInfo &info )
196{
197 cnst TWO = 2.;
198
199 fvec zeta0, zeta1, S00, S10, S11, si;
200 fvec F00, F10, F20, F30, F40, F01, F11, F21, F31, F41 ;
201 fvec K00, K10, K20, K30, K40, K01, K11, K21, K31, K41;
202
203 zeta0 = T.x - x;
204 zeta1 = T.y - y;
205
206 // F = CH'
207 F00 = T.C00;
208 F10 = T.C10;
209 F20 = T.C20;
210 F30 = T.C30;
211 F40 = T.C40;
212 F01 = T.C10;
213 F11 = T.C11;
214 F21 = T.C21;
215 F31 = T.C31;
216 F41 = T.C41;
217
218 S00 = F00 + info.C00;
219 S10 = F10 + info.C10;
220 S11 = F11 + info.C11;
221
222 si = 1./(S00*S11 - S10*S10);
223 fvec S00tmp = S00;
224 S00 = si*S11;
225 S10 = -si*S10;
226 S11 = si*S00tmp;
227
228 T.chi2+= zeta0*zeta0*S00 + 2.*zeta0*zeta1*S10 + zeta1*zeta1*S11;
229 T.NDF += TWO;
230
231 K00 = F00*S00 + F01*S10; K01 = F00*S10 + F01*S11;
232 K10 = F10*S00 + F11*S10; K11 = F10*S10 + F11*S11;
233 K20 = F20*S00 + F21*S10; K21 = F20*S10 + F21*S11;
234 K30 = F30*S00 + F31*S10; K31 = F30*S10 + F31*S11;
235 K40 = F40*S00 + F41*S10; K41 = F40*S10 + F41*S11;
236
237 T.x -= K00*zeta0 + K01*zeta1;
238 T.y -= K10*zeta0 + K11*zeta1;
239 T.tx -= K20*zeta0 + K21*zeta1;
240 T.ty -= K30*zeta0 + K31*zeta1;
241 T.qp -= K40*zeta0 + K41*zeta1;
242
243 T.C00-= K00*F00 + K01*F01;
244 T.C10-= K10*F00 + K11*F01;
245 T.C11-= K10*F10 + K11*F11;
246 T.C20-= K20*F00 + K21*F01;
247 T.C21-= K20*F10 + K21*F11;
248 T.C22-= K20*F20 + K21*F21;
249 T.C30-= K30*F00 + K31*F01;
250 T.C31-= K30*F10 + K31*F11;
251 T.C32-= K30*F20 + K31*F21;
252 T.C33-= K30*F30 + K31*F31;
253 T.C40-= K40*F00 + K41*F01;
254 T.C41-= K40*F10 + K41*F11;
255 T.C42-= K40*F20 + K41*F21;
256 T.C43-= K40*F30 + K41*F31;
257 T.C44-= K40*F40 + K41*F41;
258}
259
260
261
262/*
263inline void L1Filter1D( L1TrackPar &T, fvec &u, fvec &sigma2, fvec H[] )
264{
265
266 cnst ZERO = 0.0, ONE = 1.;
267
268 fvec wi, zeta, zetawi, HCH;
269 fvec F0, F1, F2, F3, F4;
270 fvec K1, K2, K3, K4;
271
272 zeta = H[0]*T.x +H[1]*T.y +H[2]*T.tx +H[3]*T.ty +H[4]*T.qp - u;
273
274 // F = CH'
275 F0 = H[0]*T.C00 + H[1]*T.C10 + H[2]*T.C20+ H[3]*T.C30+ H[4]*T.C40;
276 F1 = H[0]*T.C10 + H[1]*T.C11 + H[2]*T.C21+ H[3]*T.C31+ H[4]*T.C41;
277 F2 = H[0]*T.C20 + H[1]*T.C21 + H[2]*T.C22+ H[3]*T.C32+ H[4]*T.C42;
278 F3 = H[0]*T.C30 + H[1]*T.C31 + H[2]*T.C32+ H[3]*T.C33+ H[4]*T.C43;
279 F4 = H[0]*T.C40 + H[1]*T.C41 + H[2]*T.C42+ H[3]*T.C43+ H[4]*T.C44;
280
281 HCH = H[0]*F0 +H[1]*F1 +H[2]*F2 +H[3]*F3 +H[4]*F4 ;
282
283 wi = rcp(fvec(sigma2 +HCH));
284 zetawi = zeta * wi;
285 T.chi2 += zeta * zetawi;
286 T.NDF += ONE;
287
288 K1 = F1*wi;
289 K2 = F2*wi;
290 K3 = F3*wi;
291 K4 = F4*wi;
292
293 T.x -= F0*zetawi;
294 T.y -= F1*zetawi;
295 T.tx -= F2*zetawi;
296 T.ty -= F3*zetawi;
297 T.qp -= F4*zetawi;
298
299 T.C00-= F0*F0*wi;
300 T.C10-= K1*F0;
301 T.C11-= K1*F1;
302 T.C20-= K2*F0;
303 T.C21-= K2*F1;
304 T.C22-= K2*F2;
305 T.C30-= K3*F0;
306 T.C31-= K3*F1;
307 T.C32-= K3*F2;
308 T.C33-= K3*F3;
309 T.C40-= K4*F0;
310 T.C41-= K4*F1;
311 T.C42-= K4*F2;
312 T.C43-= K4*F3;
313 T.C44-= K4*F4;
314}
315
316
317inline void L1Filter2D( L1TrackPar &T, fvec &x, fvec &y, L1XYMeasurementInfo &info, fvec H[] )
318{
319 cnst TWO = 2.;
320
321 fvec zeta0, zeta1, S00, S10, S11, si;
322 fvec F00, F10, F20, F30, F40, F01, F11, F21, F31, F41 ;
323 fvec K00, K10, K20, K30, K40, K01, K11, K21, K31, K41;
324
325 zeta0 = H[0]*T.x +H[1]*T.y +H[2]*T.tx +H[3]*T.ty +H[4]*T.qp - x;
326 zeta1 = H[5]*T.x +H[6]*T.y +H[7]*T.tx +H[8]*T.ty +H[9]*T.qp - y;
327
328 // F = CH'
329 F00 = H[0]*T.C00 + H[1]*T.C10 + H[2]*T.C20+ H[3]*T.C30+ H[4]*T.C40;
330 F10 = H[0]*T.C10 + H[1]*T.C11 + H[2]*T.C21+ H[3]*T.C31+ H[4]*T.C41;
331 F20 = H[0]*T.C20 + H[1]*T.C21 + H[2]*T.C22+ H[3]*T.C32+ H[4]*T.C42;
332 F30 = H[0]*T.C30 + H[1]*T.C31 + H[2]*T.C32+ H[3]*T.C33+ H[4]*T.C43;
333 F40 = H[0]*T.C40 + H[1]*T.C41 + H[2]*T.C42+ H[3]*T.C43+ H[4]*T.C44;
334 F01 = H[5]*T.C00 + H[6]*T.C10 + H[7]*T.C20+ H[8]*T.C30+ H[9]*T.C40;
335 F11 = H[5]*T.C10 + H[6]*T.C11 + H[7]*T.C21+ H[8]*T.C31+ H[9]*T.C41;
336 F21 = H[5]*T.C20 + H[6]*T.C21 + H[7]*T.C22+ H[8]*T.C32+ H[9]*T.C42;
337 F31 = H[5]*T.C30 + H[6]*T.C31 + H[7]*T.C32+ H[8]*T.C33+ H[9]*T.C43;
338 F41 = H[5]*T.C40 + H[6]*T.C41 + H[7]*T.C42+ H[8]*T.C43+ H[9]*T.C44;
339
340 S00 = H[0]*F00 +H[1]*F10 +H[2]*F20 +H[3]*F30 +H[4]*F40 + info.C00;
341 S10 = H[5]*F00 +H[6]*F10 +H[7]*F20 +H[8]*F30 +H[9]*F40 + info.C10;
342 S11 = H[5]*F01 +H[6]*F11 +H[7]*F21 +H[8]*F31 +H[9]*F41 + info.C11;
343
344 si = rcp(fvec(S00*S11 - S10*S10));
345 fvec S00tmp = S00;
346 S00 = si*S11;
347 S10 = -si*S10;
348 S11 = si*S00tmp;
349
350 T.chi2+= zeta0*zeta0*S00 + 2.*zeta0*zeta1*S10 + zeta1*zeta1*S11;
351 T.NDF += TWO;
352
353 K00 = F00*S00 + F01*S10; K01 = F00*S10 + F01*S11;
354 K10 = F10*S00 + F11*S10; K11 = F10*S10 + F11*S11;
355 K20 = F20*S00 + F21*S10; K21 = F20*S10 + F21*S11;
356 K30 = F30*S00 + F31*S10; K31 = F30*S10 + F31*S11;
357 K40 = F40*S00 + F41*S10; K41 = F40*S10 + F41*S11;
358
359 T.x -= K00*zeta0 + K01*zeta1;
360 T.y -= K10*zeta0 + K11*zeta1;
361 T.tx -= K20*zeta0 + K21*zeta1;
362 T.ty -= K30*zeta0 + K31*zeta1;
363 T.qp -= K40*zeta0 + K41*zeta1;
364
365 T.C00-= K00*F00 + K01*F01;
366 T.C10-= K10*F00 + K11*F01;
367 T.C11-= K10*F10 + K11*F11;
368 T.C20-= K20*F00 + K21*F01;
369 T.C21-= K20*F10 + K21*F11;
370 T.C22-= K20*F20 + K21*F21;
371 T.C30-= K30*F00 + K31*F01;
372 T.C31-= K30*F10 + K31*F11;
373 T.C32-= K30*F20 + K31*F21;
374 T.C33-= K30*F30 + K31*F31;
375 T.C40-= K40*F00 + K41*F01;
376 T.C41-= K40*F10 + K41*F11;
377 T.C42-= K40*F20 + K41*F21;
378 T.C43-= K40*F30 + K41*F31;
379 T.C44-= K40*F40 + K41*F41;
380}
381
382
383
384
385inline void L1FilterFirst( fvec *T, L1Cov &C, fvec &Chi2, fvec &NDF, L1XYMeasurementInfo &info, fvec &x, fvec &y, fvec &w )
386{
387 cnst ZERO = 0.0, ONE = 1.;
388
389 fvec w1 = !w;
390 C.C00= w&info.C00 + w1&INF;
391 C.C10= w&info.C10 + w1&INF; C.C11= w&info.C11 + w1&INF;
392 C.C20= ZERO; C.C21= ZERO; C.C22= INF;
393 C.C30= ZERO; C.C31= ZERO; C.C32= ZERO; C.C33= INF;
394 C.C40= ZERO; C.C41= ZERO; C.C42= ZERO; C.C43= ZERO; C.C44= INF;
395
396 T[0] = w&x + w1&T[0];
397 T[1] = w&y + w1&T[1];
398 NDF = -3.0;
399 Chi2 = ZERO;
400}
401*/
402
403#undef cnst
404
405#endif
L1Filter
void L1Filter(L1TrackPar &T, L1UMeasurementInfo &info, fvec u, fvec w=1., fvec *du2=0)
Definition L1Filtration.h:13
L1FilterChi2XYC00C10C11
void L1FilterChi2XYC00C10C11(const L1UMeasurementInfo &info, fvec &x, fvec &y, fvec &C00, fvec &C10, fvec &C11, fvec &chi2, const fvec &u, const fvec *du2=0)
Definition L1Filtration.h:94
cnst
#define cnst
Definition L1Filtration.h:10
L1FilterXY
void L1FilterXY(L1TrackPar &T, fvec x, fvec y, L1XYMeasurementInfo &info)
Definition L1Filtration.h:195
L1FilterChi2
void L1FilterChi2(const L1UMeasurementInfo &info, const fvec &x, const fvec &y, const fvec &C00, const fvec &C10, const fvec &C11, fvec &chi2, const fvec &u, const fvec *du2=0)
Definition L1Filtration.h:74
L1FilterVtx
void L1FilterVtx(L1TrackPar &T, fvec x, fvec y, L1XYMeasurementInfo &info, fvec extrDx, fvec extrDy, fvec J[])
Definition L1Filtration.h:126