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