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