LCFIVertex  0.7.2
VertexFitterKalman.cpp
1 /*
2 
3  Kalman filter based vertex fitter
4 
5  Math implementation was adapted from S.Gorbunov and I.Kisel
6  For details see CBM-SOFT note 2007-003
7 
8  19/May/07
9  Tomas Lastovicka
10  t.lastovicka1@physics.ox.ac.uk
11 
12 */
13 
14 #include <map>
15 #include "../include/VertexFitterKalman.h"
16 #include "../include/interactionpoint.h"
17 #include "../include/vertexfitterlsm.h"
18 
19 namespace vertex_lcfi { namespace ZVTOP {
20 
21 
22  //* Track states will be sorted according to pT
23 
24  bool pDecreasing(const TState& lhs, const TState& rhs) {
25  return( fabs(lhs.track()->helixRep().invR())
26  < fabs(rhs.track()->helixRep().invR()) );
27  }
28 
29 
30  //*
31 
32  VertexFitterKalman::VertexFitterKalman() : m_useManualSeed(false), fNDF(-3), fChi2(0) {}
33 
34  void VertexFitterKalman::fitVertex(const std::vector<TrackState*> & Tracks,
35  InteractionPoint* IP,
36  Vector3 & Result,
37  Matrix3x3 & ResultError,
38  double & ChiSquaredOfFit) {
39 
40  //* Reset vertex position and covariance matrix
41 
42  for( int i=0; i<6; ++i) fP[i] = 0.;
43  for( int i=0; i<21; ++i) fC[i] = 0.;
44  fC[0] = fC[2] = fC[5] = 10000.;
45 
46  double chi2sum = 0;
47  int maxIter = 3;
48 
49 
50  //* Convert TrackStates to TStates
51 
52  fStates.clear();
53  std::vector<TrackState*>::const_iterator its;
54  for( its = Tracks.begin(); Tracks.end() != its; its++ )
55  {
56  TrackState* state = (*its);
57  TState myState(state);
58  fStates.push_back(myState);
59  }
60 
61 
62  //* For less than two tracks call LSM fitter
63  // - due to IP default error issue
64 
65  if( Tracks.size() < 2 )
66  {
67  VertexFitterLSM fitterLSM;
68  if( m_useManualSeed ) fitterLSM.setSeed(m_manualSeed);
69  std::map<TrackState*,double> ChiSquaredOfTracks;
70  double ChiSquaredOfIP;
71  fitterLSM.fitVertex(Tracks, IP, Result, ResultError,
72  ChiSquaredOfFit, ChiSquaredOfTracks, ChiSquaredOfIP);
73  fP[0] = Result.x();
74  fP[1] = Result.y();
75  fP[2] = Result.z();
76  fC[0] = ResultError(0,0);
77  fC[1] = ResultError(0,1);
78  fC[2] = ResultError(1,1);
79  fC[3] = ResultError(0,2);
80  fC[4] = ResultError(1,2);
81  fC[5] = ResultError(2,2);
82  return;
83  }
84 
85 
86  //* Sort track states according to transverse momentum
87 
88  std::sort( fStates.begin(), fStates.end(), pDecreasing );
89 
90 
91  //* Set initial vertex position guess (for linearisation)
92 
93  fVtxGuess[0] = 0.; fVtxGuess[1] = 0.; fVtxGuess[2] = 0.;
94  fNDF = -3; fChi2 = 0;
95 
96  if (m_useManualSeed) {
97  fVtxGuess[0] = m_manualSeed(0);
98  fVtxGuess[1] = m_manualSeed(1);
99  fVtxGuess[2] = m_manualSeed(2);
100  }
101  else
102  {
103  if( fStates.size()>1 )
104  {
105  bool flag = estimateVertex(fVtxGuess);
106  if( !flag )
107  {
108  fVtxGuess[0] = 0.; fVtxGuess[1] = 0.; fVtxGuess[2] = 0.;
109  }
110  }
111  if( fStates.size()==1 || fStates.size() == 0 )
112  {
113  if( IP ) {
114  fVtxGuess[0] = IP->position().x();
115  fVtxGuess[1] = IP->position().y();
116  fVtxGuess[2] = IP->position().z();
117  // fill already here for no track case
118  fC[0] = IP->errorMatrix()(0,0);
119  fC[1] = IP->errorMatrix()(0,1);
120  fC[2] = IP->errorMatrix()(1,1);
121  fC[3] = IP->errorMatrix()(0,2);
122  fC[4] = IP->errorMatrix()(1,2);
123  fC[5] = IP->errorMatrix()(2,2);
124  }
125  }
126  }
127 
128  if( fStates.size() == 0 || (fStates.size() == 1 && !IP) ) {
129  Result(0) = fVtxGuess[0];
130  Result(1) = fVtxGuess[1];
131  Result(2) = fVtxGuess[2];
132  ResultError(0,0) = fC[0];
133  ResultError(0,1) = ResultError(1,0) = fC[1];
134  ResultError(1,1) = fC[2];
135  ResultError(0,2) = ResultError(2,0) = fC[3];
136  ResultError(1,2) = ResultError(2,1) = fC[4];
137  ResultError(2,2) = fC[5];
138  ChiSquaredOfFit = fChi2;
139  return;
140  }
141 
142  //* Initial vertex position
143 
144  fP[0] = fVtxGuess[0]; fP[1] = fVtxGuess[1]; fP[2] = fVtxGuess[2];
145 
146  if( IP ) {
147  fP[0] = IP->position().x();
148  fP[1] = IP->position().y();
149  fP[2] = IP->position().z();
150  fC[0] = IP->errorMatrix()(0,0);
151  fC[1] = IP->errorMatrix()(0,1);
152  fC[2] = IP->errorMatrix()(1,1);
153  fC[3] = IP->errorMatrix()(0,2);
154  fC[4] = IP->errorMatrix()(1,2);
155  fC[5] = IP->errorMatrix()(2,2);
156  }
157 
158  //* Loop over TStates - add them one by one
159 
160  for( std::vector<TState>::iterator it = fStates.begin(); fStates.end() != it; it++ )
161  {
162 
163  TState* state = &(*it);
164 
165  for( int iter=0; iter<maxIter; iter++ )
166  {
167 
168  double *ffP = fP, *ffC = fC;
169  double m[6], mV[21];
170 
171  state->GetMeasurement( fVtxGuess, m, mV );
172 
173  //*
174 
175  double mS[6];
176  {
177  double mSi[6] = { ffC[0]+mV[0],
178  ffC[1]+mV[1], ffC[2]+mV[2],
179  ffC[3]+mV[3], ffC[4]+mV[4], ffC[5]+mV[5] };
180 
181  mS[0] = mSi[2]*mSi[5] - mSi[4]*mSi[4];
182  mS[1] = mSi[3]*mSi[4] - mSi[1]*mSi[5];
183  mS[2] = mSi[0]*mSi[5] - mSi[3]*mSi[3];
184  mS[3] = mSi[1]*mSi[4] - mSi[2]*mSi[3];
185  mS[4] = mSi[1]*mSi[3] - mSi[0]*mSi[4];
186  mS[5] = mSi[0]*mSi[2] - mSi[1]*mSi[1];
187 
188  double s = ( mSi[0]*mS[0] + mSi[1]*mS[1] + mSi[3]*mS[3] );
189  s = ( s > 1.E-20 ) ? 1./s : 0 ;
190 
191  mS[0]*=s; mS[1]*=s; mS[2]*=s;
192  mS[3]*=s; mS[4]*=s; mS[5]*=s;
193  }
194 
195  //* Residual (measured - estimated)
196 
197  double zeta[3] = { m[0]-ffP[0], m[1]-ffP[1], m[2]-ffP[2] };
198 
199  //* CHt = CH' - D'
200 
201  double mCHt0[6], mCHt1[6], mCHt2[6];
202 
203  mCHt0[0]=ffC[ 0] ; mCHt1[0]=ffC[ 1] ; mCHt2[0]=ffC[ 3] ;
204  mCHt0[1]=ffC[ 1] ; mCHt1[1]=ffC[ 2] ; mCHt2[1]=ffC[ 4] ;
205  mCHt0[2]=ffC[ 3] ; mCHt1[2]=ffC[ 4] ; mCHt2[2]=ffC[ 5] ;
206  mCHt0[3]=ffC[ 6]-mV[ 6]; mCHt1[3]=ffC[ 7]-mV[ 7]; mCHt2[3]=ffC[ 8]-mV[ 8];
207  mCHt0[4]=ffC[10]-mV[10]; mCHt1[4]=ffC[11]-mV[11]; mCHt2[4]=ffC[12]-mV[12];
208  mCHt0[5]=ffC[15]-mV[15]; mCHt1[5]=ffC[16]-mV[16]; mCHt2[5]=ffC[17]-mV[17];
209 
210  //* Kalman gain K = mCH'*S
211 
212  double k0[6], k1[6], k2[6];
213 
214  for(int i=0;i<6;++i){
215  k0[i] = mCHt0[i]*mS[0] + mCHt1[i]*mS[1] + mCHt2[i]*mS[3];
216  k1[i] = mCHt0[i]*mS[1] + mCHt1[i]*mS[2] + mCHt2[i]*mS[4];
217  k2[i] = mCHt0[i]*mS[3] + mCHt1[i]*mS[4] + mCHt2[i]*mS[5];
218  }
219 
220  //* New estimation of the vertex position
221 
222  if( iter<maxIter-1 ){
223  for(int i=0; i<3; ++i)
224  fVtxGuess[i]= ffP[i] + k0[i]*zeta[0]+k1[i]*zeta[1]+k2[i]*zeta[2];
225  continue;
226  }
227 
228  // last iteration -> update the particle
229 
230  //* Add the daughter momentum to the particle momentum
231 
232  ffP[ 3] += m[ 3];
233  ffP[ 4] += m[ 4];
234  ffP[ 5] += m[ 5];
235 
236  ffC[ 9] += mV[ 9];
237  ffC[13] += mV[13];
238  ffC[14] += mV[14];
239  ffC[18] += mV[18];
240  ffC[19] += mV[19];
241  ffC[20] += mV[20];
242 
243 
244  //* New estimation of the vertex position r += K*zeta
245 
246  for( int i=0; i<6; ++i )
247  fP[i] = ffP[i] + k0[i]*zeta[0] + k1[i]*zeta[1] + k2[i]*zeta[2];
248 
249 
250  //* New covariance matrix C -= K*(mCH')'
251 
252  for(int i=0,k=0; i<6; ++i) {
253  for(int j=0; j<=i; ++j,++k)
254  fC[k] = ffC[k] - (k0[i]*mCHt0[j] + k1[i]*mCHt1[j] + k2[i]*mCHt2[j] );
255  }
256 
257 
258  //* Calculate Chi^2
259 
260  fChi2 += (mS[0]*zeta[0] + mS[1]*zeta[1] + mS[3]*zeta[2])*zeta[0]
261  + (mS[1]*zeta[0] + mS[2]*zeta[1] + mS[4]*zeta[2])*zeta[1]
262  + (mS[3]*zeta[0] + mS[4]*zeta[1] + mS[5]*zeta[2])*zeta[2];
263 
264  fNDF += 2;
265  }
266  }
267 
268 
269  //* Recalculate Chi2 (although Kalman filter Chi2 is fine)
270 
271  chi2sum = 0;
272  fChi2chain.clear(); // for potential event re-weighting
273 
274  for( std::vector<TState>::iterator it = fStates.begin();
275  fStates.end() != it; it++ )
276  {
277  TState* state = &(*it);
278  double chi2 = getDeviationFromVertex( state, fP, fC );
279  fChi2chain.push_back(chi2);
280  chi2sum += chi2;
281  }
282 
283  Result(0) = fP[0]; Result(1) = fP[1]; Result(2) = fP[2];
284 
285  ResultError(0,0) = fC[0];
286  ResultError(0,1) = ResultError(1,0) = fC[1];
287  ResultError(1,1) = fC[2];
288  ResultError(0,2) = ResultError(2,0) = fC[3];
289  ResultError(1,2) = ResultError(2,1) = fC[4];
290  ResultError(2,2) = fC[5];
291 
292 
293  //* Add Chi2 if IP was used
294 
295  if( IP ) chi2sum += IP->chi2(Result);
296 
297  fChi2 = chi2sum;
298  ChiSquaredOfFit = fChi2;
299 
300  }
301 
302 
303  void VertexFitterKalman::fitVertex(const std::vector<TrackState*> & Tracks,
304  InteractionPoint* IP, Vector3 & Result)
305  {
306  Matrix3x3 ResultError; double ChiSquaredOfFit;
307  this->fitVertex(Tracks, IP, Result, ResultError, ChiSquaredOfFit);
308  }
309 
310 
311  void VertexFitterKalman::fitVertex(const std::vector<TrackState*> & Tracks,
312  InteractionPoint* IP,
313  Vector3 & Result, double & ChiSquaredOfFit)
314  {
315  Matrix3x3 ResultError;
316  this->fitVertex(Tracks, IP, Result, ResultError, ChiSquaredOfFit);
317  }
318 
319 
320  void VertexFitterKalman::fitVertex(const std::vector<TrackState*> & Tracks,
321  InteractionPoint* IP,
322  Vector3 & Result, double & ChiSquaredOfFit,
323  std::map<TrackState*,double> & ChiSquaredOfTrack,
324  double & ChiSquaredOfIP)
325  {
326  Matrix3x3 ResultError;
327  this->fitVertex(Tracks, IP, Result, ResultError, ChiSquaredOfFit,
328  ChiSquaredOfTrack, ChiSquaredOfIP);
329 
330  }
331 
332 
333  void VertexFitterKalman::fitVertex(const std::vector<TrackState*> & Tracks,
334  InteractionPoint* IP,
335  Vector3 & Result, Matrix3x3 & ResultError,
336  double & ChiSquaredOfFit,
337  std::map<TrackState*,double> & ChiSquaredOfTrack,
338  double & ChiSquaredOfIP) {
339 
340  this->fitVertex(Tracks, IP, Result, ResultError, ChiSquaredOfFit);
341 
342  ChiSquaredOfTrack.clear();
343  std::vector<TrackState*>::const_iterator its;
344  for( its = Tracks.begin(); Tracks.end() != its; its++ )
345  {
346  TState myState(*its);
347  TState* state = &myState;
348  double chi2 = getDeviationFromVertex( state, fP, fC );
349  ChiSquaredOfTrack.insert( std::pair<TrackState*,double>( (*its), chi2 ) );
350  }
351 
352  ChiSquaredOfIP = 0;
353  if( IP ) ChiSquaredOfIP = IP->chi2(Result);
354 
355  }
356 
357  // -----------------------------------------------------------------------------------
358 
359 
360  double VertexFitterKalman::getDeviationFromVertex( const TState* state,
361  const double v[],
362  const double Cv[] ) const
363  {
364  //* Calculate Chi2 deviation from vertex
365  //* v = [xyz], Cv=[Cxx,Cxy,Cyy,Cxz,Cyz,Czz]-covariance matrix
366 
367  double mP[6]; double mC[21];
368 
369  state->TransportBz( state->GetDStoPointBz(v), mP, mC );
370 
371  double d[3]={ v[0]-mP[0], v[1]-mP[1], v[2]-mP[2] };
372 
373  double sigmaS = 0.1 + 10.*sqrt( (d[0]*d[0]+d[1]*d[1]+d[2]*d[2])/
374  (mP[3]*mP[3]+mP[4]*mP[4]+mP[5]*mP[5]) );
375 
376  double h[3] = { mP[3]*sigmaS, mP[4]*sigmaS, mP[5]*sigmaS };
377 
378  double mSi[6] =
379  { mC[0] +h[0]*h[0],
380  mC[1] +h[1]*h[0], mC[2] +h[1]*h[1],
381  mC[3] +h[2]*h[0], mC[4] +h[2]*h[1], mC[5] +h[2]*h[2] };
382 
383  if( Cv ){
384  mSi[0]+=Cv[0];
385  mSi[1]+=Cv[1];
386  mSi[2]+=Cv[2];
387  mSi[3]+=Cv[3];
388  mSi[4]+=Cv[4];
389  mSi[5]+=Cv[5];
390  }
391 
392  double mS[6];
393 
394  mS[0] = mSi[2]*mSi[5] - mSi[4]*mSi[4];
395  mS[1] = mSi[3]*mSi[4] - mSi[1]*mSi[5];
396  mS[2] = mSi[0]*mSi[5] - mSi[3]*mSi[3];
397  mS[3] = mSi[1]*mSi[4] - mSi[2]*mSi[3];
398  mS[4] = mSi[1]*mSi[3] - mSi[0]*mSi[4];
399  mS[5] = mSi[0]*mSi[2] - mSi[1]*mSi[1];
400 
401  double s = ( mSi[0]*mS[0] + mSi[1]*mS[1] + mSi[3]*mS[3] );
402  s = ( s > 1.E-20 ) ? 1./s : 0;
403 
404  return fabs(s*( ( mS[0]*d[0] + mS[1]*d[1] + mS[3]*d[2])*d[0]
405  +(mS[1]*d[0] + mS[2]*d[1] + mS[4]*d[2])*d[1]
406  +(mS[3]*d[0] + mS[4]*d[1] + mS[5]*d[2])*d[2] ));
407  }
408 
409  void VertexFitterKalman::setSeed(Vector3 Seed)
410  {
411  m_useManualSeed = true;
412  m_manualSeed = Seed;
413  }
414 
415  bool VertexFitterKalman::estimateVertex(double vtx[]) {
416  std::vector<TState>::iterator it1, it2;
417  std::vector<double> vx, vy, vz;
418  for ( it1 = fStates.begin(); fStates.end() != it1+1; it1++ ) {
419  TState* state1 = &(*it1);
420  for ( it2 = it1+1; fStates.end() != it2; it2++ ) {
421  TState* state2 = &(*it2);
422  double ds1, ds2;
423  bool flag = state1->GetDStoTStateBz(state2, ds1, ds2);
424  if( !flag ) continue;
425  double m1[6], m2[6], V1[25], V2[25];
426  state1->TransportBz( ds1, m1, V1 );
427  state2->TransportBz( ds2, m2, V2 );
428  double xg = .5*( m1[0] + m2[0] );
429  double yg = .5*( m1[1] + m2[1] );
430  double zg = .5*( m1[2] + m2[2] );
431  vx.push_back(xg);
432  vy.push_back(yg);
433  vz.push_back(zg);
434  }
435  }
436  if( vx.size() == 0 ) return false;
437  vtx[0] = robustMean(vx);
438  vtx[1] = robustMean(vy);
439  vtx[2] = robustMean(vz);
440  return true;
441  }
442 
443  double VertexFitterKalman::robustMean(std::vector<double> vec)
444  {
445  int size = vec.size();
446  if( size == 0 ) return 0;
447  if( size == 1 ) return vec[0];
448 
449  double mean = 0, rms = 0;
450  double rmean = 0, wsum = 0;
451 
452  for( int i = 0; size != i; i++ ) mean += vec[i];
453  mean /= size;
454 
455  for( int i = 0; size != i; i++ )
456  rms += (vec[i]-mean)*(vec[i]-mean);
457  rms /= size;
458 
459  for( int i = 0; size != i; i++ ) {
460  double dif = vec[i]-mean;
461  double weight = exp(-(dif*dif)/(2.0*rms));
462  rmean += weight * vec[i];
463  wsum += weight;
464  }
465  rmean /= wsum;
466  return rmean;
467  }
468 
469  }
470 }
471 
vertex_lcfi::util::Vector3
Multi-Perpose 3 Vector.
Definition: include/vertex_lcfi/util/inc/vector3.h:33
vertex_lcfi::util::Matrix3x3
Definition: include/vertex_lcfi/util/inc/matrix.h:74
Generated on Fri Dec 2 2016 12:36:49 for LCFIVertex by   doxygen 1.8.6