CbmRoot
CbmKFPixelMeasurement.cxx
Go to the documentation of this file.
1 #include "CbmKFPixelMeasurement.h"
2 
3 #include <cmath>
4 
5 using std::vector;
6 
7 ClassImp(CbmKFPixelMeasurement);
8 
9 Int_t CbmKFPixelMeasurement::Filter(CbmKFTrackInterface& track) {
10 
11  Bool_t err = 0;
12 
13  Double_t* T = track.GetTrack();
14  Double_t* C = track.GetCovMatrix();
15 
16  Double_t w = (C[0] + V[0]) * (C[2] + V[2]) - (C[1] + V[1]) * (C[1] + V[1]);
17  if (w < 1.E-20 || !finite(w)) {
18  err = 1;
19  return err;
20  }
21  w = 1. / w;
22  if (!finite(w)) return 1;
23 
24  Double_t W[3] = {w * (C[2] + V[2]), -w * (C[1] + V[1]), w * (C[0] + V[0])};
25  Double_t zeta[2] = {T[0] - x, T[1] - y};
26 
27  track.GetRefChi2() += zeta[0] * zeta[0] * W[0] + 2 * zeta[0] * zeta[1] * W[1]
28  + zeta[1] * zeta[1] * W[2];
29  track.GetRefNDF() += 2;
30 
31  Double_t K[5][2] = {
32  {C[0] * W[0] + C[1] * W[1], C[0] * W[1] + C[1] * W[2]},
33  {C[1] * W[0] + C[2] * W[1], C[1] * W[1] + C[2] * W[2]},
34  {C[3] * W[0] + C[4] * W[1], C[3] * W[1] + C[4] * W[2]},
35  {C[6] * W[0] + C[7] * W[1], C[6] * W[1] + C[7] * W[2]},
36  {C[10] * W[0] + C[11] * W[1], C[10] * W[1] + C[11] * W[2]},
37  };
38 
39  T[0] -= K[0][0] * zeta[0] + K[0][1] * zeta[1];
40  T[1] -= K[1][0] * zeta[0] + K[1][1] * zeta[1];
41  T[2] -= K[2][0] * zeta[0] + K[2][1] * zeta[1];
42  T[3] -= K[3][0] * zeta[0] + K[3][1] * zeta[1];
43  T[4] -= K[4][0] * zeta[0] + K[4][1] * zeta[1];
44 
45  Double_t KHC[15] = {C[0] * K[0][0] + C[1] * K[0][1],
46  C[0] * K[1][0] + C[1] * K[1][1],
47  C[1] * K[1][0] + C[2] * K[1][1],
48 
49  C[0] * K[2][0] + C[1] * K[2][1],
50  C[1] * K[2][0] + C[2] * K[2][1],
51  C[3] * K[2][0] + C[4] * K[2][1],
52 
53  C[0] * K[3][0] + C[1] * K[3][1],
54  C[1] * K[3][0] + C[2] * K[3][1],
55  C[3] * K[3][0] + C[4] * K[3][1],
56  C[6] * K[3][0] + C[7] * K[3][1],
57 
58  C[0] * K[4][0] + C[1] * K[4][1],
59  C[1] * K[4][0] + C[2] * K[4][1],
60  C[3] * K[4][0] + C[4] * K[4][1],
61  C[6] * K[4][0] + C[7] * K[4][1],
62  C[10] * K[4][0] + C[11] * K[4][1]};
63 
64  C[0] -= KHC[0];
65  C[1] -= KHC[1];
66  C[2] -= KHC[2];
67  C[3] -= KHC[3];
68  C[4] -= KHC[4];
69  C[5] -= KHC[5];
70  C[6] -= KHC[6];
71  C[7] -= KHC[7];
72  C[8] -= KHC[8];
73  C[9] -= KHC[9];
74  C[10] -= KHC[10];
75  C[11] -= KHC[11];
76  C[12] -= KHC[12];
77  C[13] -= KHC[13];
78  C[14] -= KHC[14];
79  return 0;
80 }
81 
82 
84 //
85 // mathAddMeasurements: the implementation of the Probabilistic
86 // Data Association Filter for the MAPS
87 //
88 //
89 // Author : Dmitry Emeliyanov, RAL, dmitry.emeliyanov@cern.ch
90 //
92 
93 
94 //#define _DEBUG_PDAF_
95 
96 void CbmKFPixelMeasurement::FilterPDAF(CbmKFTrackInterface& track,
97  vector<CbmKFPixelMeasurement*>& vm,
98  double gateX,
99  double gateY,
100  vector<double>& vProb) {
101  const double Pdetect = 0.90; // hit efficiency
102  const double gateEff =
103  0.99; // probability of the correct hit falling into the search window
104  // 10 sigma x 10 sigma size
105 
106  Double_t* T = track.GetTrack();
107  Double_t* C = track.GetCovMatrix();
108 
109  double gateArea = gateX * gateY;
110  double chi2, zeta[2];
111  int ndf = 2, idx, i, j, k;
112  vector<double> vExp;
113  vector<double> vResidX;
114  vector<double> vResidY;
115  vector<double> vChi2;
116 
117 #ifdef _DEBUG_PDAF_
118 
119  cout << "PDAF: " << vm->size() << " hits validated" << endl;
120  cout << "Initial params: x=" << T[0] << " y=" << T[1] << " tx=" << T[2]
121  << " ty=" << T[3] << " Q=" << T[4] << endl;
122  cout << "GateX=" << gateX << " GateY=" << gateY << endl;
123 
124 #endif
125 
126  vExp.clear();
127  vResidX.clear();
128  vResidY.clear();
129  vChi2.clear();
130  double Sk, Ck;
131 
132  const double* V = (*vm.begin())->V;
133 
134  // determinant of the residual covariance
135 
136  Sk = (C[0] + V[0]) * (C[2] + V[2]) - (C[1] + V[1]) * (C[1] + V[1]);
137 
138  // 1. Normalization constant
139 
140  double Lambda = 1. / 1.;
141 
142  Ck = 2.0 * 3.1415926 * sqrt(Sk) * (1.0 - Pdetect * gateEff)
143  / (gateArea * Pdetect * gateEff) * Lambda;
144 
145  double w = Sk;
146  if (w < 1.E-20) return;
147  w = 1. / w;
148 
149  // 2. Kalman gain - assumed to be the same for all gathered hits in a window
150 
151  double W[3] = {w * (C[2] + V[2]), -w * (C[1] + V[1]), w * (C[0] + V[0])};
152  double K[5][2] = {
153  {C[0] * W[0] + C[1] * W[1], C[0] * W[1] + C[1] * W[2]},
154  {C[1] * W[0] + C[2] * W[1], C[1] * W[1] + C[2] * W[2]},
155  {C[3] * W[0] + C[4] * W[1], C[3] * W[1] + C[4] * W[2]},
156  {C[6] * W[0] + C[7] * W[1], C[6] * W[1] + C[7] * W[2]},
157  {C[10] * W[0] + C[11] * W[1], C[10] * W[1] + C[11] * W[2]},
158  };
159 
160  // 3. Exponential weights
161 
162  for (vector<CbmKFPixelMeasurement*>::iterator pmIt = vm.begin();
163  pmIt != vm.end();
164  ++pmIt) {
165  double resX = (*pmIt)->x - T[0];
166  double resY = (*pmIt)->y - T[1];
167  chi2 = resX * resX * W[0] + 2 * resY * resX * W[1] + resY * resY * W[2];
168 #ifdef _DEBUG_PDAF_
169  cout << "resX=" << resX << " resY=" << resY << " chi2dist=" << chi2 << endl;
170 #endif
171  vChi2.push_back(chi2);
172  vResidX.push_back(resX);
173  vResidY.push_back(resY);
174  vExp.push_back(exp(-0.5 * chi2));
175  }
176 
177  // 4. Assignment probabilities
178 
179  double Sum = Ck;
180  vector<double>::iterator eIt;
181  for (eIt = vExp.begin(); eIt != vExp.end(); ++eIt) {
182  Sum += (*eIt);
183  }
184  Sum = 1.0 / Sum;
185  double P0 = Ck * Sum;
186 #ifdef _DEBUG_PDAF_
187  cout << "No-detect probability = " << P0 << endl;
188 #endif
189  for (eIt = vExp.begin(); eIt != vExp.end(); ++eIt) {
190  vProb.push_back(Sum * (*eIt));
191  }
192 
193  // 5. Merged (i.e. weighted by the assignment probabilities) residuals
194 
195  zeta[0] = 0.0;
196  zeta[1] = 0.0;
197  idx = 0;
198  chi2 = 0.0;
199 
200  for (eIt = vProb.begin(); eIt != vProb.end(); ++eIt) {
201  zeta[0] += vResidX[idx] * (*eIt);
202  zeta[1] += vResidY[idx] * (*eIt);
203  chi2 += vChi2[idx] * (*eIt);
204 #ifdef _DEBUG_PDAF_
205  cout << "Hit " << idx << " Assignment probability = " << (*eIt) << endl;
206 #endif
207  idx++;
208  }
209 #ifdef _DEBUG_PDAF_
210  cout << "Merged resX=" << zeta[0] << " resY=" << zeta[1] << endl;
211 #endif
212 
213  // 6. Empirical correlation matrix of the residuals
214 
215  double CR[2][2];
216  CR[0][0] = -zeta[0] * zeta[0];
217  CR[0][1] = -zeta[0] * zeta[1];
218  CR[1][0] = -zeta[1] * zeta[0];
219  CR[1][1] = -zeta[1] * zeta[1];
220  idx = 0;
221  for (eIt = vProb.begin(); eIt != vProb.end(); ++eIt) {
222  CR[0][0] += (*eIt) * vResidX[idx] * vResidX[idx];
223  CR[0][1] += (*eIt) * vResidX[idx] * vResidY[idx];
224  CR[1][0] += (*eIt) * vResidY[idx] * vResidX[idx];
225  CR[1][1] += (*eIt) * vResidY[idx] * vResidY[idx];
226  idx++;
227  }
228  double KD[5][2];
229  for (i = 0; i < 5; i++)
230  for (j = 0; j < 2; j++) {
231  KD[i][j] = 0.0;
232  for (k = 0; k < 2; k++)
233  KD[i][j] += K[i][k] * CR[k][j];
234  }
235 
236  // 7. Additional covariance term - reflects influence of the residual spread
237 
238  double Ga[5][5];
239  for (i = 0; i < 5; i++)
240  for (j = 0; j < 5; j++) {
241  Ga[i][j] = 0.0;
242  for (k = 0; k < 2; k++)
243  Ga[i][j] += KD[i][k] * K[j][k];
244  }
245 
246  // 8. Track parameters update - usual Kalman formalizm but with the merged
247  // residual
248 
249  T[0] += K[0][0] * zeta[0] + K[0][1] * zeta[1];
250  T[1] += K[1][0] * zeta[0] + K[1][1] * zeta[1];
251  T[2] += K[2][0] * zeta[0] + K[2][1] * zeta[1];
252  T[3] += K[3][0] * zeta[0] + K[3][1] * zeta[1];
253  T[4] += K[4][0] * zeta[0] + K[4][1] * zeta[1];
254 
255  double KHC[15] = {C[0] * K[0][0] + C[1] * K[0][1],
256  C[0] * K[1][0] + C[1] * K[1][1],
257  C[1] * K[1][0] + C[2] * K[1][1],
258 
259  C[0] * K[2][0] + C[1] * K[2][1],
260  C[1] * K[2][0] + C[2] * K[2][1],
261  C[3] * K[2][0] + C[4] * K[2][1],
262 
263  C[0] * K[3][0] + C[1] * K[3][1],
264  C[1] * K[3][0] + C[2] * K[3][1],
265  C[3] * K[3][0] + C[4] * K[3][1],
266  C[6] * K[3][0] + C[7] * K[3][1],
267 
268  C[0] * K[4][0] + C[1] * K[4][1],
269  C[1] * K[4][0] + C[2] * K[4][1],
270  C[3] * K[4][0] + C[4] * K[4][1],
271  C[6] * K[4][0] + C[7] * K[4][1],
272  C[10] * K[4][0] + C[11] * K[4][1]};
273 
274  double Gs[15];
275  for (i = 0; i < 15; i++)
276  Gs[i] = C[i];
277 
278  // 9. Standard Kalman covariance
279 
280  Gs[0] -= KHC[0];
281  Gs[1] -= KHC[1];
282  Gs[2] -= KHC[2];
283  Gs[3] -= KHC[3];
284  Gs[4] -= KHC[4];
285  Gs[5] -= KHC[5];
286  Gs[6] -= KHC[6];
287  Gs[7] -= KHC[7];
288  Gs[8] -= KHC[8];
289  Gs[9] -= KHC[9];
290  Gs[10] -= KHC[10];
291  Gs[11] -= KHC[11];
292  Gs[12] -= KHC[12];
293  Gs[13] -= KHC[13];
294  Gs[14] -= KHC[14];
295 
296  // 10. PDAF covariance: linear combination of the extrapolated covariance and
297  // covariance produced by the Kalman filter + additional term
298 
299  idx = 0;
300  for (i = 0; i < 5; i++)
301  for (j = 0; j <= i; j++) {
302  C[idx] = P0 * C[idx] + (1 - P0) * Gs[idx] + Ga[i][j];
303  idx++;
304  }
305 #ifdef _DEBUG_PDAF_
306  cout << "Updated params: x=" << T[0] << " y=" << T[1] << " tx=" << T[2]
307  << " ty=" << T[3] << " Q=" << T[4] << endl;
308  cout << "chi2 contrib.=" << chi2 << endl;
309 #endif
310 
311  track.GetRefChi2() += chi2;
312  track.GetRefNDF() += ndf;
313 }
exp
friend F32vec4 exp(const F32vec4 &a)
Definition: L1/vectors/P4_F32vec4.h:134
CbmKFPixelMeasurement::x
Double_t x
Definition: CbmKFPixelMeasurement.h:24
CbmKFTrackInterface::GetTrack
virtual Double_t * GetTrack()
Is it electron.
Definition: CbmKFTrackInterface.cxx:33
sqrt
friend F32vec4 sqrt(const F32vec4 &a)
Definition: L1/vectors/P4_F32vec4.h:41
CbmKFPixelMeasurement::FilterPDAF
static void FilterPDAF(CbmKFTrackInterface &track, std::vector< CbmKFPixelMeasurement * > &vm, double gateX, double gateY, std::vector< double > &vProb)
Definition: CbmKFPixelMeasurement.cxx:96
i
int i
Definition: L1/vectors/P4_F32vec4.h:25
CbmKFPixelMeasurement
Definition: CbmKFPixelMeasurement.h:18
finite
T finite(T x)
Definition: CbmL1Def.h:21
CbmKFPixelMeasurement::y
Double_t y
Definition: CbmKFPixelMeasurement.h:25
CbmKFPixelMeasurement.h
ClassImp
ClassImp(CbmKFPixelMeasurement)
CbmKFTrackInterface::GetRefNDF
virtual Int_t & GetRefNDF()
Chi^2 after fit.
Definition: CbmKFTrackInterface.cxx:36
CbmKFTrackInterface
Definition: CbmKFTrackInterface.h:26
CbmKFTrackInterface::GetCovMatrix
virtual Double_t * GetCovMatrix()
array[6] of track parameters(x,y,tx,ty,qp,z)
Definition: CbmKFTrackInterface.cxx:34
CbmKFPixelMeasurement::Filter
Int_t Filter(CbmKFTrackInterface &track)
Definition: CbmKFPixelMeasurement.cxx:9
CbmKFPixelMeasurement::V
Double_t V[3]
Definition: CbmKFPixelMeasurement.h:26
CbmKFTrackInterface::GetRefChi2
virtual Double_t & GetRefChi2()
array[15] of covariance matrix
Definition: CbmKFTrackInterface.cxx:35