ATLAS Offline Software
GsfMeasurementUpdator.cxx
Go to the documentation of this file.
1 /*
2  Copyright (C) 2002-2024 CERN for the benefit of the ATLAS collaboration
3 */
12 //
16 //
20 
21 #include "CxxUtils/inline_hints.h"
22 
23 #include <memory>
24 
25 namespace {
26 
27 // constants
28 const Trk::ProjectionMatricesSet s_reMatrices(5);
29 constexpr double s_thetaGainDampingValue = 0.1;
30 const AmgVector(5) s_cov0Vec {250., 250., 0.25, 0.25, 0.000001};
31 const AmgMatrix(5, 5) s_unitMatrix(AmgMatrix(5, 5)::Identity());
32 const AmgVector(2) s_thetaMin(0.0, -M_PI);
33 enum RangeCheckDef
34 {
35  absoluteCheck = 0,
37 };
38 
39 struct componentsCache
40 {
41  struct element
42  {
43  double determinantR;
44  double chi2;
45  };
46  std::array<element, GSFConstants::maxNumberofStateComponents> elements{};
47  size_t numElements = 0;
48 };
49 
51 int measurementCoord_1D(int paramKey) {
52  for (int i = 0; i < 5; ++i) {
53  if (paramKey & (1 << i)) {
54  return i;
55  }
56  }
57 
58  return 0;
59 }
60 
67 inline bool
68 thetaPhiWithinRange_5D(const AmgVector(5) & V, const RangeCheckDef rcd)
69 {
70  return ((std::abs(V(Trk::phi)) <= M_PI) &&
71  (V(Trk::theta) >= s_thetaMin(rcd)) && (V(Trk::theta) <= M_PI));
72 }
73 
77 inline bool
78 thetaWithinRange_5D(const AmgVector(5) & V)
79 {
80  return (V(Trk::theta) >= 0.0 && (V(Trk::theta) <= M_PI));
81 }
82 
83 bool
84 correctThetaPhiRange_5D(AmgVector(5) & V,
85  AmgSymMatrix(5) & M,
86  const RangeCheckDef rcd)
87 {
88 
89  // correct theta coordinate
90  if (V(Trk::theta) < s_thetaMin((int)rcd) || V(Trk::theta) > M_PI) {
91  // absolute theta: repair if between -pi and +2pi.
92  // differential theta: repair if between -pi and +pi
93  if (V(Trk::theta) < -M_PI) {
94  return false;
95  }
96  if (V(Trk::theta) > (rcd == differentialCheck ? M_PI : 2 * M_PI)) {
97  return false;
98  }
99  if (V(Trk::theta) > M_PI) {
100  V(Trk::theta) = 2 * M_PI - V(Trk::theta);
101  V(Trk::phi) += (V(Trk::phi) > 0.0) ? -M_PI : M_PI;
102  } else if (V(Trk::theta) < 0.0) {
103  V(Trk::theta) = -V(Trk::theta);
104  V(Trk::phi) += (V(Trk::phi) > 0.0) ? -M_PI : M_PI;
105  }
106  // correct also cov matrix:
107  M.fillSymmetric(0, 3, -M(0, 3)); // cov(polar,locX)
108  M.fillSymmetric(1, 3, -M(1, 3)); // cov(polar,locY)
109  M.fillSymmetric(2, 3, -M(2, 3)); // cov(polar,azimuthal)
110  M.fillSymmetric(3, 4, -M(3, 4)); // cov(polar,qOverP)
111  }
112 
113  // correct phi coordinate if necessary
114  if ((V(Trk::phi) > M_PI)) {
115  V(Trk::phi) = fmod(V(Trk::phi) + M_PI, 2 * M_PI) - M_PI;
116  } else if ((V(Trk::phi) < -M_PI)) {
117  V(Trk::phi) = fmod(V(Trk::phi) - M_PI, 2 * M_PI) + M_PI;
118  }
119 
120  return true;
121 }
122 
127 template<int DIM>
128 AmgSymMatrix(DIM) projection_T(const AmgSymMatrix(5) & M, int key)
129 {
130  if (key == 3 || key == 7 || key == 15) { // shortcuts for the most common use
131  // cases
132  return M.topLeftCorner<DIM, DIM>();
133  }
134 
135  Eigen::Matrix<int, DIM, 1, 0, DIM, 1> iv;
136  iv.setZero();
137  for (int i = 0, k = 0; i < 5; ++i) {
138  iv[k++] = key & (1 << i) ? i : 0;
139  }
140 
141  AmgSymMatrix(DIM) covSubMatrix;
142  covSubMatrix.setZero();
143  for (int i = 0; i < DIM; ++i) {
144  for (int j = 0; j < DIM; ++j) {
145  covSubMatrix(i, j) = M(iv(i), iv(j));
146  }
147  }
148 
149  return covSubMatrix;
150 }
151 
160 bool
161 calculateFilterStep_1D(Trk::TrackParameters& TP,
162  const AmgSymMatrix(5) & trkCov,
163  double measPar,
164  double measCov,
165  int paramKey,
166  Trk::FitQualityOnSurface& fitQoS)
167 {
168  const int mk = measurementCoord_1D(paramKey);
169  // get the parameters from the
170  const AmgVector(5)& trkPar = TP.parameters();
171  // use measuring coordinate (variable "mk") instead of reduction matrix
172  const double r = measPar - trkPar(mk);
173  double R = measCov + trkCov(mk, mk);
174  if (R == 0.0) {
175  return false;
176  }
177  R = 1. / R;
178  // --- compute Kalman gain matrix
179  AmgVector(5) K = trkCov.col(mk) * R;
180  // --- compute local filtered state, here = TP+K*r = TP + TCov * H.T * R * r
181  AmgVector(5) newPar = trkPar + K * r;
182 
183  if (!thetaWithinRange_5D(newPar)) {
184 
185  if (mk != Trk::theta &&
186  (std::abs(R * r) > 1.0 ||
187  trkCov(Trk::theta, Trk::theta) > 0.1 * s_cov0Vec(Trk::theta))) {
188 
189  AmgVector(5) dampedCov = trkCov.col(mk);
190 
191  dampedCov(Trk::theta) = dampedCov(Trk::theta) * s_thetaGainDampingValue;
192 
193  newPar = trkPar + dampedCov * R * r;
194 
195  K(Trk::theta, 0) = K(Trk::theta, 0) * s_thetaGainDampingValue;
196  }
197  }
198  // --- compute covariance matrix of local filteres state
199  AmgMatrix(5, 5) KtimesH;
200  KtimesH.setZero();
201  KtimesH.col(mk) = K;
202  const AmgMatrix(5, 5) M = s_unitMatrix - KtimesH;
203  AmgSymMatrix(5) newCov =
204  trkCov.similarity(M) + K * measCov * K.transpose();
205 
206  if (!thetaPhiWithinRange_5D(newPar, absoluteCheck) &&
207  !correctThetaPhiRange_5D(newPar, newCov, absoluteCheck)) {
208  return false;
209  }
210 
211  const double predictedResidual = (measPar - newPar(mk));
212  const AmgSymMatrix(5)& updatedCov = newCov;
213 
214  double chiSquared = measCov - updatedCov(mk, mk);
215  if (chiSquared != 0.0) {
216  // get chi2 = r.T() * R^-1 * r
217  chiSquared = predictedResidual * predictedResidual / chiSquared;
218  }
220  // In place update of parameters
221  TP.updateParameters(newPar, newCov);
222  return true;
223 }
224 
225 bool
226 calculateFilterStep_5D(Trk::TrackParameters& TP,
227  const AmgSymMatrix(5) & trkCov,
228  const AmgVector(5) & measPar,
229  const AmgSymMatrix(5) & measCov,
231 {
232  // get the parameter vector
233  const AmgVector(5)& trkPar = TP.parameters();
234  // Kalman gain K, residual r, combined covariance R
235  const AmgVector(5) r = measPar - trkPar;
236  // for full safety in Eigen see
237  // http://eigen.tuxfamily.org/dox/classEigen_1_1FullPivLU.html
238  const AmgSymMatrix(5) R = (measCov + trkCov).inverse();
239  const AmgMatrix(5, 5) K = trkCov * R;
240  const AmgMatrix(5, 5) M = s_unitMatrix - K;
241  // --- compute local filtered state
242  const AmgVector(5) newPar = trkPar + K * r;
243  // --- compute filtered covariance matrix
244  const AmgSymMatrix(5) newCov =
245  trkCov.similarity(M) + measCov.similarity(K);
246  const double chiSquared = Amg::chi2(R, r);
247  // create the FQSonSurface
248  fQ = Trk::FitQualityOnSurface(chiSquared, 5);
249  TP.updateParameters(newPar, newCov);
250  return true;
251 }
252 
253 template<int DIM>
254 bool
255 calculateFilterStep_T(Trk::TrackParameters& TP,
256  const AmgSymMatrix(5) & trkCov,
257  const AmgVector(DIM) & measPar,
258  const AmgSymMatrix(DIM) & measCov,
259  int paramKey,
260  Trk::FitQualityOnSurface& fQ)
261 {
262 
263  // get the parameter vector
264  const AmgVector(5)& trkPar = TP.parameters();
265  // reduction matrix
266  const AmgMatrix(DIM, 5) H =
267  s_reMatrices.expansionMatrix(paramKey).topLeftCorner<DIM, 5>();
268  // the projected parameters from the TrackParameters
269  AmgVector(DIM) projTrkPar;
270  if (paramKey == 3 || paramKey == 7 || paramKey == 15) {
271  projTrkPar = trkPar.head<DIM>();
272  } else {
273  projTrkPar = H * trkPar;
274  }
275 
276  // reduction matrix H, Kalman gain K, residual r, combined covariance R
277  // residual after reduction
278  const AmgVector(DIM) r = measPar - projTrkPar;
279  // combined covariance after reduction
280  const AmgSymMatrix(DIM) R =
281  (measCov + projection_T<DIM>(trkCov, paramKey)).inverse();
282  // Kalman gain matrix
283  const AmgMatrix(5, DIM) K = trkCov * H.transpose() * R;
284  const AmgMatrix(5, 5) M = s_unitMatrix - K * H;
285  // --- compute local filtered state
286  const AmgVector(5) newPar = trkPar + K * r;
287  // --- compute filtered covariance matrix
288  // C = M * trkCov * M.T() + K * covRio * K.T()
289  const AmgSymMatrix(5) newCov =
290  trkCov.similarity(M) + K * measCov * K.transpose();
291  const double chiSquared = Amg::chi2(R, r);
292  // create the FQSonSurface
293  fQ = Trk::FitQualityOnSurface(chiSquared, DIM);
294  // In place update of parameters
295  TP.updateParameters(newPar, newCov);
296  return true;
297 }
298 
299 /*
300  * Method implementing the actual kalman filter step
301  * using the methods above.
302  * Also does heavy use of Eigen
303  * so we use flatten
304  */
306 bool
307 filterStep(Trk::TrackParameters& trackParameters,
308  Trk::FitQualityOnSurface& fitQos,
309  const Trk::LocalParameters& measurement,
310  const Amg::MatrixX& measCovariance)
311 {
312 
313  const AmgSymMatrix(5)* trkCov = trackParameters.covariance();
314  if (!trkCov) {
315  return false;
316  }
317 
318  const int nLocCoord = measCovariance.cols();
319  if (!(measurement.dimension() == nLocCoord)) {
320  return false;
321  }
322 
323  switch (nLocCoord) {
324  case 1: {
325  return calculateFilterStep_1D(trackParameters,
326  *trkCov,
327  measurement(0),
328  measCovariance(0, 0),
329  measurement.parameterKey(),
330  fitQos);
331  }
332  case 2: {
333  return calculateFilterStep_T<2>(trackParameters,
334  *trkCov,
335  measurement.head<2>(),
336  measCovariance.topLeftCorner<2, 2>(),
337  measurement.parameterKey(),
338  fitQos);
339  }
340  case 3: {
341  return calculateFilterStep_T<3>(trackParameters,
342  *trkCov,
343  measurement.head<3>(0),
344  measCovariance.topLeftCorner<3, 3>(),
345  measurement.parameterKey(),
346  fitQos);
347  }
348  case 4: {
349  return calculateFilterStep_T<4>(trackParameters,
350  *trkCov,
351  measurement.head<4>(0),
352  measCovariance.topLeftCorner<4, 4>(),
353  measurement.parameterKey(),
354  fitQos);
355  }
356  case 5: {
357  return calculateFilterStep_5D(trackParameters,
358  *trkCov,
359  measurement.head<5>(),
360  measCovariance.topLeftCorner<5, 5>(),
361  fitQos);
362  }
363  default: {
364  return false;
365  }
366  }
367 }
368 
369 /*
370  * Methods needed to calculate the
371  * fit quality
372  */
373 bool
374 makeChi2_1D(Trk::FitQualityOnSurface& updatedFitQoS,
375  const AmgVector(5) & trkPar,
376  const AmgSymMatrix(5) & trkCov,
377  double valRio,
378  double rioCov,
379  int paramKey)
380 {
381 
382  const int mk = measurementCoord_1D(paramKey);
383  const double r = valRio - trkPar(mk);
384  // if (mk==3) catchPiPi;
385  double chiSquared = rioCov + trkCov(mk, mk);
386  if (chiSquared == 0.0) {
387  return false;
388  }
389  chiSquared = r * r / chiSquared;
390  updatedFitQoS = Trk::FitQualityOnSurface(chiSquared, 1);
391  return true;
392 }
393 
394 template<int DIM>
395 bool
396 makeChi2_T(Trk::FitQualityOnSurface& updatedFitQoS,
397  const AmgVector(5) & trkPar,
398  const AmgSymMatrix(5) & trkCov,
399  const AmgVector(DIM) & measPar,
400  const AmgSymMatrix(DIM) & covPar,
401  int paramKey)
402 
403 {
404 
405  const AmgMatrix(DIM, 5) H =
406  s_reMatrices.expansionMatrix(paramKey).topLeftCorner<DIM, 5>();
407  const AmgVector(DIM) r = measPar - H * trkPar;
408  // get the projected matrix
409  AmgSymMatrix(DIM) R = projection_T<DIM>(trkCov, paramKey);
410  R += covPar;
411  // calcualte the chi2 value
412  const double chiSquared = Amg::chi2(R.inverse(), r);
413  updatedFitQoS = Trk::FitQualityOnSurface(chiSquared, DIM);
414  return true;
415 }
416 
417 bool
418 stateFitQuality(Trk::FitQualityOnSurface& updatedFitQoS,
419  const Trk::TrackParameters& trkPar,
420  const Trk::LocalParameters& position,
421  const Amg::MatrixX& covariance)
422 {
423  if (!trkPar.covariance()) {
424  return false;
425  }
426  // For the LocalPos. version, need to get # meas. coord. from covariance
427  // matrix.
428  const int nLocCoord = covariance.cols();
429  switch (nLocCoord) {
430  case 1: {
431  return makeChi2_1D(updatedFitQoS,
432  trkPar.parameters(),
433  (*trkPar.covariance()),
434  position[Trk::locX],
435  covariance(0, 0),
436  1);
437  }
438  case 2: {
439  return makeChi2_T<2>(updatedFitQoS,
440  trkPar.parameters(),
441  (*trkPar.covariance()),
442  position,
443  covariance.topLeftCorner<2, 2>(),
444  3);
445  }
446  default: {
447  return false;
448  }
449  }
450 }
451 
452 /*
453  * Methods to adjust the weights of the
454  * multicomponent state (Gaussian sum)
455  * given a measurement.
456  */
457 template<int DIM>
458 std::pair<double, double>
459 calculateWeight_T(const Trk::TrackParameters* componentTrackParameters,
460  const AmgSymMatrix(5) * predictedCov,
461  const AmgVector(DIM) & measPar,
462  const AmgSymMatrix(DIM) & measCov,
463  int paramKey)
464 {
465  // Define the expansion matrix
466  const AmgMatrix(DIM, 5) H =
467  s_reMatrices.expansionMatrix(paramKey).topLeftCorner<DIM, 5>();
468 
469  // Calculate the residual
470  const AmgVector(DIM) r = measPar - H * componentTrackParameters->parameters();
471  // Residual covariance. Posterior weights is calculated used predicted state
472  // and measurement. Therefore add covariances
473  AmgSymMatrix(DIM) const R(measCov + predictedCov->similarity(H));
474  // compute determinant of residual
475  const double det = R.determinant();
476  if (det == 0) {
477  return { 0, 0 };
478  }
479  // Compute Chi2
480  return {det, (1. / (double)DIM) * Amg::chi2(R.inverse(), r)};
481 }
482 
483 std::pair<double, double>
484 calculateWeight_1D(const Trk::TrackParameters* componentTrackParameters,
485  const AmgSymMatrix(5) * predictedCov,
486  const double measPar,
487  const double measCov,
488  int paramKey)
489 {
490  const int mk = measurementCoord_1D(paramKey);
491  // Calculate the residual
492  const double r = measPar - (componentTrackParameters->parameters())(mk);
493  // Residual covariance. Posterior weights is calculated used predicted state
494  // and measurement. Therefore add covariances
495  const double R = measCov + (*predictedCov)(mk, mk);
496  if (R == 0) {
497  return { 0, 0 };
498  }
499  // Compute Chi2
500  return { R, r * r / R };
501 }
502 
503 std::pair<double, double>
504 calculateWeight_2D_3(const Trk::TrackParameters* componentTrackParameters,
505  const AmgSymMatrix(5) * predictedCov,
506  const AmgVector(2) & measPar,
507  const AmgSymMatrix(2) & measCov)
508 {
509  // Calculate the residual
510  const AmgVector(2) r =
511  measPar - componentTrackParameters->parameters().head<2>();
512  // Residual covariance. Posterior weights is calculated used predicted state
513  // and measurement. Therefore add covariances
514  const AmgSymMatrix(2) R(measCov + predictedCov->topLeftCorner<2, 2>());
515  // compute determinant of residual
516  const double det = R.determinant();
517  if (det == 0) {
518  return { 0, 0 };
519  }
520  // Compute Chi2
521  return {det, 0.5 * Amg::chi2(R.inverse(), r)};
522 }
523 
525 weights(Trk::MultiComponentState&& predictedState,
526  const Trk::MeasurementBase& measurement)
527 {
528 
529  const size_t predictedStateSize = predictedState.size();
530  if (predictedStateSize == 0) {
531  return {};
532  }
533  if (predictedStateSize > GSFConstants::maxNumberofStateComponents) {
534  throw std::runtime_error("PosteriorWeightsCalculator :Invalid predictedState size");
535  }
536  const Trk::LocalParameters& measurementLocalParameters =
537  measurement.localParameters();
538  const int nLocCoord = measurement.localParameters().dimension();
539  if (nLocCoord < 1 || nLocCoord > 5) {
540  return {};
541  }
542 
543  // Move to output and update
544  Trk::MultiComponentState returnMultiComponentState = std::move(predictedState);
545 
546  // Calculate chi2 and determinant of each component.
547  componentsCache determinantRandChi2{};
548  double minimumChi2(10.e10); // Initalise high
549  // Loop over all components
550  for (const auto& component : returnMultiComponentState) {
551 
552  const Trk::TrackParameters* componentTrackParameters =
553  component.params.get();
554  if (!componentTrackParameters) {
555  continue;
556  }
557  const AmgSymMatrix(5)* predictedCov =
558  componentTrackParameters->covariance();
559  if (!predictedCov) {
560  continue;
561  }
562 
563  std::pair<double, double> result(0, 0);
564  switch (nLocCoord) {
565  case 1: {
566  result = calculateWeight_1D(componentTrackParameters,
567  predictedCov,
568  measurementLocalParameters(0),
569  measurement.localCovariance()(0, 0),
570  measurementLocalParameters.parameterKey());
571  } break;
572  case 2: {
573  if (measurementLocalParameters.parameterKey() == 3) {
574  result = calculateWeight_2D_3(
575  componentTrackParameters,
576  predictedCov,
577  measurementLocalParameters.head<2>(),
578  measurement.localCovariance().topLeftCorner<2, 2>());
579  } else {
580  result = calculateWeight_T<2>(
581  componentTrackParameters,
582  predictedCov,
583  measurementLocalParameters.head<2>(),
584  measurement.localCovariance().topLeftCorner<2, 2>(),
585  measurementLocalParameters.parameterKey());
586  }
587  } break;
588  case 3: {
589  result =
590  calculateWeight_T<3>(componentTrackParameters,
591  predictedCov,
592  measurementLocalParameters.head<3>(),
593  measurement.localCovariance().topLeftCorner<3, 3>(),
594  measurementLocalParameters.parameterKey());
595  } break;
596  case 4: {
597  result =
598  calculateWeight_T<4>(componentTrackParameters,
599  predictedCov,
600  measurementLocalParameters.head<4>(),
601  measurement.localCovariance().topLeftCorner<4, 4>(),
602  measurementLocalParameters.parameterKey());
603  } break;
604  case 5: {
605  result =
606  calculateWeight_T<5>(componentTrackParameters,
607  predictedCov,
608  measurementLocalParameters.head<5>(),
609  measurement.localCovariance().topLeftCorner<5, 5>(),
610  measurementLocalParameters.parameterKey());
611  } break;
612  default: {
613  }
614  }
615 
616  if (result.first == 0) {
617  continue;
618  }
619  // Cache R and Chi2
620  determinantRandChi2.elements[determinantRandChi2.numElements] = {
621  result.first, result.second
622  };
623  ++determinantRandChi2.numElements;
624  if (result.second < minimumChi2) {
625  minimumChi2 = result.second;
626  }
627  } // end loop over components
628 
629  // If something went wrong in the loop return empty
630  if (determinantRandChi2.numElements != predictedStateSize) {
631  return {};
632  }
633 
634  // Calculate posterior weights.
635  size_t index(0);
636  double sumWeights(0.);
637  std::array<double, GSFConstants::maxNumberofStateComponents> fallBackWeights{};
638  auto componentItr = returnMultiComponentState.begin();
639  for (; componentItr != returnMultiComponentState.end();
640  ++componentItr, ++index) {
641  // Extract common factor to avoid numerical problems during exponentiation
642  const double chi2 = determinantRandChi2.elements[index].chi2 - minimumChi2;
643  const double priorWeight = componentItr->weight;
644  fallBackWeights[index] = priorWeight;
645  double updatedWeight(0.);
646  // Determinant can not be below 1e-19. Rather ugly but protect
647  // against 0 determinants Normally occur when the component is a poor fit
648  if (determinantRandChi2.elements[index].determinantR > 1e-20) {
649  updatedWeight =
650  priorWeight *
651  sqrt(1. / determinantRandChi2.elements[index].determinantR) *
652  exp(-0.5 * chi2);
653  } else {
654  updatedWeight = 1e-10;
655  }
656  componentItr->weight = updatedWeight;
657  sumWeights += updatedWeight;
658  }
659  if (sumWeights > 0.) {
660  const double invertSumWeights = 1. / sumWeights;
661  // Renormalise the state to total weight = 1
662  for (auto& returnComponent : returnMultiComponentState) {
663  returnComponent.weight *= invertSumWeights;
664  }
665  } else {
666  // If the sum weights is less than 0 revert them back
667  size_t fallbackIndex(0);
668  for (auto& returnComponent : returnMultiComponentState) {
669  returnComponent.weight = fallBackWeights[fallbackIndex];
670  ++fallbackIndex;
671  }
672  }
673  return returnMultiComponentState;
674 }
675 
676 /*
677  * Methods to handle cases with invalid
678  * components
679  */
680 bool
681 invalidComponent(const Trk::TrackParameters* trackParameters)
682 {
683  const auto* measuredCov = trackParameters->covariance();
684  if (!measuredCov) {
685  return true;
686  }
687 
688  for (int i = 0; i < 5; ++i) {
689  if ((*measuredCov)(i, i) <= 0.) {
690  return true;
691  }
692  }
693 
694  return false;
695 }
696 
698 rebuildState(Trk::MultiComponentState&& stateBeforeUpdate)
699 {
700  Trk::MultiComponentState stateWithInsertedErrors =
701  std::move(stateBeforeUpdate);
702  // We need to loop checking for invalid componets i.e negative covariance
703  // diagonal elements and update them with a large covariance matrix
704  for (const Trk::ComponentParameters& component : stateWithInsertedErrors) {
705  const Trk::TrackParameters* trackParameters = component.params.get();
706  const bool rebuildCov = invalidComponent(trackParameters);
707  if (rebuildCov) {
708  AmgSymMatrix(5) bigNewCovarianceMatrix = AmgSymMatrix(5)::Zero();
709  const double covarianceScaler = 1.;
710  bigNewCovarianceMatrix(0, 0) = 250. * covarianceScaler;
711  bigNewCovarianceMatrix(1, 1) = 250. * covarianceScaler;
712  bigNewCovarianceMatrix(2, 2) = 0.25;
713  bigNewCovarianceMatrix(3, 3) = 0.25;
714  bigNewCovarianceMatrix(4, 4) = 0.001 * 0.001;
715  component.params->updateParameters(trackParameters->parameters(),
716  bigNewCovarianceMatrix);
717  }
718  }
719  return stateWithInsertedErrors;
720 }
721 
722 /*
723  * Methods that bring all
724  * weights adjustement, filter step,
725  * and FitQuality together,
726  * so as to update the current
727  * multi-component state given a measurement
728  */
730 calculateFilterStep(Trk::MultiComponentState&& stateBeforeUpdate,
731  const Trk::MeasurementBase& measurement,
732  Trk::FitQualityOnSurface& fitQoS)
733 {
734  // state Assembler cache
736  if (stateBeforeUpdate.empty()) {
737  return {};
738  }
739 
740  // Calculate the weight of each component after the measurement
741  Trk::MultiComponentState stateWithNewWeights =
742  weights(std::move(stateBeforeUpdate), measurement);
743 
744  if (stateWithNewWeights.empty()) {
745  return {};
746  }
747 
748  double chiSquared = 0;
749  int degreesOfFreedom = 0;
750  for (Trk::ComponentParameters& component : stateWithNewWeights) {
751  if (stateWithNewWeights.size() > 1 &&
752  std::abs(component.params->parameters()[Trk::qOverP]) > 0.033333) {
753  continue;
754  }
755  Trk::FitQualityOnSurface componentFitQuality;
757  bool const updateSuccess = filterStep(*(component.params),
758  componentFitQuality,
759  measurement.localParameters(),
760  measurement.localCovariance());
761  if (!updateSuccess) {
762  continue;
763  }
764 
765  if (invalidComponent(component.params.get())) {
766  continue;
767  }
768 
769  if (componentFitQuality.chiSquared() <= 0.) {
770  continue;
771  }
772 
773  chiSquared += component.weight * componentFitQuality.chiSquared();
774 
775  // The same measurement is included in each update
776  // so we can update the degree of freedom only
777  if (degreesOfFreedom == 0.0) {
778  degreesOfFreedom = componentFitQuality.numberDoF();
779  }
780 
781  // Add component to state being prepared for assembly
783  std::move(component));
784  }
785 
786  Trk::MultiComponentState assembledUpdatedState =
788 
789  if (assembledUpdatedState.empty()) {
790  return {};
791  }
792 
793  fitQoS.setChiSquared(chiSquared);
794  fitQoS.setNumberDoF(degreesOfFreedom);
795  // Renormalise state
797  return assembledUpdatedState;
798 }
799 
800 } // end of anonymous namespace
801 
802 // The external/interface methods
803 // implemented in terms of the internal ones.
806  const Trk::MeasurementBase& measurement,
807  FitQualityOnSurface& fitQoS)
808 {
809 
810  // Check all components have associated error matrices
811  bool rebuildStateWithErrors = false;
812  // Perform initial check of state awaiting update. If all states have
813  // associated error matricies then no need to perform the rebuild
814  for (const Trk::ComponentParameters& component : stateBeforeUpdate) {
815  rebuildStateWithErrors =
816  rebuildStateWithErrors || invalidComponent(component.params.get());
817  }
818 
819  if (rebuildStateWithErrors) {
820  Trk::MultiComponentState stateWithInsertedErrors =
821  rebuildState(std::move(stateBeforeUpdate));
822  // Perform the measurement update with the modified state
823  Trk::MultiComponentState updatedState = calculateFilterStep(
824  std::move(stateWithInsertedErrors), measurement, fitQoS);
825  if (updatedState.empty()) {
826  return {};
827  }
828  return updatedState;
829  }
830 
831  // Perform the measurement update
832  Trk::MultiComponentState updatedState = calculateFilterStep(
833  std::move(stateBeforeUpdate), measurement, fitQoS);
834 
835  if (updatedState.empty()) {
836  return {};
837  }
838  return updatedState;
839 }
840 
843  const MeasurementBase& measurement)
844 {
845  double chi2 = 0;
846  Trk::FitQualityOnSurface componentFitQuality;
847  for (const Trk::ComponentParameters& component: updatedState) {
848  stateFitQuality(componentFitQuality,
849  *component.params.get(),
850  measurement.localParameters(),
851  measurement.localCovariance());
852 
853  chi2 += component.weight * componentFitQuality.chiSquared();
854  }
855 
856  //The same measurement is included
857  return { chi2, componentFitQuality.numberDoF() };
858 }
859 
AllowedVariables::e
e
Definition: AsgElectronSelectorTool.cxx:37
beamspotman.r
def r
Definition: beamspotman.py:674
Trk::LocalParameters
Definition: LocalParameters.h:98
Amg::MatrixX
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > MatrixX
Dynamic Matrix - dynamic allocation.
Definition: EventPrimitives.h:27
inline_hints.h
get_generator_info.result
result
Definition: get_generator_info.py:21
MeasurementBase.h
Trk::MultiComponentStateAssembler::assembledState
MultiComponentState assembledState(MultiComponentStateAssembler::Cache &&cache)
Method to return the cached state object - it performs a reweighting before returning the object base...
Definition: MultiComponentStateAssembler.cxx:120
Trk::differentialCheck
@ differentialCheck
Definition: KalmanUpdatorSMatrix.h:58
Trk::locX
@ locX
Definition: ParamDefs.h:37
Trk::LocalParameters::parameterKey
int parameterKey() const
Identifier key for matrix expansion/reduction.
TRTCalib_Extractor.det
det
Definition: TRTCalib_Extractor.py:36
index
Definition: index.py:1
Trk::ProjectionMatricesSet
the matrices to access the variably-dimensioned local parameters and map them to the defined five tra...
Definition: ProjectionMatricesSet.h:29
theta
Scalar theta() const
theta method
Definition: AmgMatrixBasePlugin.h:75
taskman.template
dictionary template
Definition: taskman.py:316
M_PI
#define M_PI
Definition: ActiveFraction.h:11
Trk::MultiComponentStateAssembler::Cache
Definition: MultiComponentStateAssembler.h:35
const
bool const RAWDATA *ch2 const
Definition: LArRodBlockPhysicsV0.cxx:560
drawFromPickle.exp
exp
Definition: drawFromPickle.py:36
Trk::ParametersCommon::updateParameters
void updateParameters(const AmgVector(DIM) &, const AmgSymMatrix(DIM) &)
Update parameters and covariance , passing covariance by ref.
AmgSymMatrix
#define AmgSymMatrix(dim)
Definition: EventPrimitives.h:50
AmgMatrix
#define AmgMatrix(rows, cols)
Definition: EventPrimitives.h:49
similarity
Matrix< Scalar, OtherDerived::RowsAtCompileTime, OtherDerived::RowsAtCompileTime > similarity(const MatrixBase< OtherDerived > &m) const
similarity method : yields ms = m*s*m^T
Definition: AmgMatrixBasePlugin.h:133
xAOD::TrackParameters
TrackParameters_v1 TrackParameters
Definition: Event/xAOD/xAODTracking/xAODTracking/TrackParameters.h:11
GSFConstants::maxNumberofStateComponents
constexpr int8_t maxNumberofStateComponents
The state is described by N Gaussian components The Beth Heitler Material effect are also described b...
Definition: GsfConstants.h:43
Trk::FitQualityOnSurface
Definition: FitQualityOnSurface.h:19
MultiComponentStateAssembler.h
H
#define H(x, y, z)
Definition: MD5.cxx:114
lumiFormat.i
int i
Definition: lumiFormat.py:85
ATH_FLATTEN
#define ATH_FLATTEN
Definition: inline_hints.h:52
Trk::theta
@ theta
Definition: ParamDefs.h:66
MuonR4::inverse
CalibratedSpacePoint::Covariance_t inverse(const CalibratedSpacePoint::Covariance_t &mat)
Inverts the parsed matrix.
Definition: MuonSpectrometer/MuonPhaseII/Event/MuonSpacePoint/src/UtilFunctions.cxx:65
AmgVector
AmgVector(4) T2BSTrackFilterTool
Definition: T2BSTrackFilterTool.cxx:114
chi2
double chi2(TH1 *h0, TH1 *h1)
Definition: comparitor.cxx:525
Trk::MultiComponentState
std::vector< ComponentParameters > MultiComponentState
Definition: ComponentParameters.h:27
AnalysisUtils::Delta::R
double R(const INavigable4Momentum *p1, const double v_eta, const double v_phi)
Definition: AnalysisMisc.h:49
Trk::ParametersBase
Definition: ParametersBase.h:55
xAOD::double
double
Definition: CompositeParticle_v1.cxx:159
Trk::FitQualityOnSurface::numberDoF
int numberDoF() const
returns the number of degrees of freedom of the overall track or vertex fit as integer
Definition: FitQuality.h:60
Trk::RangeCheckDef
RangeCheckDef
Definition: KalmanUpdatorSMatrix.h:56
Trk::MeasurementBase::localCovariance
const Amg::MatrixX & localCovariance() const
Interface method to get the localError.
Definition: MeasurementBase.h:138
Trk::MeasurementBase
Definition: MeasurementBase.h:58
head
std::string head(std::string s, const std::string &pattern)
head of a string
Definition: computils.cxx:312
Trk
Ensure that the ATLAS eigen extensions are properly loaded.
Definition: FakeTrackBuilder.h:9
Amg
Definition of ATLAS Math & Geometry primitives (Amg)
Definition: AmgStringHelpers.h:19
Trk::MultiComponentStateHelpers::renormaliseState
void renormaliseState(MultiComponentState &, double norm=1)
Performing renormalisation of total state weighting to one.
Definition: ComponentParameters.cxx:80
TRT::Track::degreesOfFreedom
@ degreesOfFreedom
Definition: InnerDetector/InDetCalibEvent/TRT_CalibData/TRT_CalibData/TrackInfo.h:79
Trk::absoluteCheck
@ absoluteCheck
Definition: KalmanUpdatorSMatrix.h:57
weights
Definition: herwig7_interface.h:38
Trk::ComponentParameters
Definition: ComponentParameters.h:22
LocalParameters.h
Trk::MeasurementBase::localParameters
const LocalParameters & localParameters() const
Interface method to get the LocalParameters.
Definition: MeasurementBase.h:132
IDTPM::chiSquared
float chiSquared(const U &p)
Definition: TrackParametersHelper.h:128
Trk::GsfMeasurementUpdator::fitQuality
FitQualityOnSurface fitQuality(const MultiComponentState &, const MeasurementBase &)
Method for determining the chi2 of the multi-component state and the number of degrees of freedom.
Definition: GsfMeasurementUpdator.cxx:842
Trk::LocalParameters::dimension
int dimension() const
Dimension of this localParameters() vector.
EventPrimitivesCovarianceHelpers.h
DeMoScan.index
string index
Definition: DeMoScan.py:362
Trk::qOverP
@ qOverP
perigee
Definition: ParamDefs.h:67
if
if(febId1==febId2)
Definition: LArRodBlockPhysicsV0.cxx:567
physics_parameters.parameters
parameters
Definition: physics_parameters.py:144
GsfConstants.h
Trk::phi
@ phi
Definition: ParamDefs.h:75
Trk::GsfMeasurementUpdator::update
MultiComponentState update(Trk::MultiComponentState &&, const Trk::MeasurementBase &, FitQualityOnSurface &fitQoS)
Method for updating the multi-state with a new measurement and calculate the fit qaulity at the same ...
Definition: GsfMeasurementUpdator.cxx:805
PowhegControl_ttFCNC_NLO.params
params
Definition: PowhegControl_ttFCNC_NLO.py:226
FitQuality.h
GsfMeasurementUpdator.h
Code for performing kalman filter update step on multi-component states for the gaussian-sum filter.
Trk::MultiComponentStateAssembler::addComponent
void addComponent(MultiComponentStateAssembler::Cache &cache, ComponentParameters &&multiComponentState)
Method to add a single set of Trk::ComponentParameters to the cached Trk::MultiComponentState object ...
Definition: MultiComponentStateAssembler.h:71
Trk::FitQualityOnSurface::chiSquared
double chiSquared() const
returns the of the overall track fit
Definition: FitQuality.h:56
fitman.k
k
Definition: fitman.py:528
generate::Zero
void Zero(TH1D *hin)
Definition: generate.cxx:32
Amg::chi2
double chi2(const T &precision, const U &residual, const int sign=1)
Definition: EventPrimitivesCovarianceHelpers.h:221
mapkey::key
key
Definition: TElectronEfficiencyCorrectionTool.cxx:37