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