29 constexpr
double s_thetaGainDampingValue = 0.1;
30 const AmgVector(5) s_cov0Vec {250., 250., 0.25, 0.25, 0.000001};
39 struct componentsCache
46 std::array<element, GSFConstants::maxNumberofStateComponents> elements{};
47 size_t numElements = 0;
51 int measurementCoord_1D(
int paramKey) {
52 for (
int i = 0;
i < 5; ++
i) {
53 if (paramKey & (1 <<
i)) {
78 thetaWithinRange_5D(
const AmgVector(5) & V)
107 M.fillSymmetric(0, 3, -M(0, 3));
108 M.fillSymmetric(1, 3, -M(1, 3));
109 M.fillSymmetric(2, 3, -M(2, 3));
110 M.fillSymmetric(3, 4, -M(3, 4));
132 return M.topLeftCorner<DIM, DIM>();
135 Eigen::Matrix<int, DIM, 1, 0, DIM, 1> iv;
137 for (
int i = 0,
k = 0;
i < 5; ++
i) {
138 iv[
k++] =
key & (1 <<
i) ?
i : 0;
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));
168 const int mk = measurementCoord_1D(paramKey);
170 const AmgVector(5)& trkPar = TP.parameters();
172 const double r = measPar - trkPar(mk);
173 double R = measCov + trkCov(mk, mk);
183 if (!thetaWithinRange_5D(newPar)) {
186 (std::abs(
R *
r) > 1.0 ||
193 newPar = trkPar + dampedCov *
R *
r;
204 trkCov.
similarity(M) + K * measCov * K.transpose();
211 const double predictedResidual = (measPar - newPar(mk));
214 double chiSquared = measCov - updatedCov(mk, mk);
233 const AmgVector(5)& trkPar = TP.parameters();
249 TP.updateParameters(newPar, newCov);
260 Trk::FitQualityOnSurface& fQ)
264 const AmgVector(5)& trkPar = TP.parameters();
267 s_reMatrices.expansionMatrix(paramKey).topLeftCorner<DIM, 5>();
270 if (paramKey == 3 || paramKey == 7 || paramKey == 15) {
271 projTrkPar = trkPar.head<DIM>();
273 projTrkPar =
H * trkPar;
278 const AmgVector(DIM)
r = measPar - projTrkPar;
281 (measCov + projection_T<DIM>(trkCov, paramKey)).
inverse();
283 const AmgMatrix(5, DIM) K = trkCov *
H.transpose() *
R;
284 const AmgMatrix(5, 5) M = s_unitMatrix - K *
H;
290 trkCov.
similarity(M) + K * measCov * K.transpose();
295 TP.updateParameters(newPar, newCov);
308 Trk::FitQualityOnSurface& fitQos,
309 const Trk::LocalParameters& measurement,
313 const AmgSymMatrix(5)* trkCov = trackParameters.covariance();
318 const int nLocCoord = measCovariance.cols();
319 if (!(measurement.dimension() == nLocCoord)) {
325 return calculateFilterStep_1D(trackParameters,
328 measCovariance(0, 0),
329 measurement.parameterKey(),
333 return calculateFilterStep_T<2>(trackParameters,
335 measurement.head<2>(),
336 measCovariance.topLeftCorner<2, 2>(),
337 measurement.parameterKey(),
341 return calculateFilterStep_T<3>(trackParameters,
343 measurement.head<3>(0),
344 measCovariance.topLeftCorner<3, 3>(),
345 measurement.parameterKey(),
349 return calculateFilterStep_T<4>(trackParameters,
351 measurement.head<4>(0),
352 measCovariance.topLeftCorner<4, 4>(),
353 measurement.parameterKey(),
357 return calculateFilterStep_5D(trackParameters,
359 measurement.head<5>(),
360 measCovariance.topLeftCorner<5, 5>(),
382 const int mk = measurementCoord_1D(paramKey);
383 const double r = valRio - trkPar(mk);
406 s_reMatrices.expansionMatrix(paramKey).topLeftCorner<DIM, 5>();
418 stateFitQuality(
Trk::FitQualityOnSurface& updatedFitQoS,
420 const Trk::LocalParameters& position,
423 if (!trkPar.covariance()) {
428 const int nLocCoord = covariance.cols();
431 return makeChi2_1D(updatedFitQoS,
433 (*trkPar.covariance()),
439 return makeChi2_T<2>(updatedFitQoS,
441 (*trkPar.covariance()),
443 covariance.topLeftCorner<2, 2>(),
458 std::pair<double, double>
467 s_reMatrices.expansionMatrix(paramKey).topLeftCorner<DIM, 5>();
483 std::pair<double, double>
486 const double measPar,
487 const double measCov,
490 const int mk = measurementCoord_1D(paramKey);
492 const double r = measPar - (componentTrackParameters->parameters())(mk);
495 const double R = measCov + (*predictedCov)(mk, mk);
500 return {
R,
r *
r /
R };
503 std::pair<double, double>
529 const size_t predictedStateSize = predictedState.size();
530 if (predictedStateSize == 0) {
534 throw std::runtime_error(
"PosteriorWeightsCalculator :Invalid predictedState size");
539 if (nLocCoord < 1 || nLocCoord > 5) {
547 componentsCache determinantRandChi2{};
548 double minimumChi2(10.e10);
550 for (
const auto& component : returnMultiComponentState) {
553 component.params.get();
554 if (!componentTrackParameters) {
558 componentTrackParameters->covariance();
563 std::pair<double, double>
result(0, 0);
566 result = calculateWeight_1D(componentTrackParameters,
568 measurementLocalParameters(0),
574 result = calculateWeight_2D_3(
575 componentTrackParameters,
577 measurementLocalParameters.head<2>(),
580 result = calculateWeight_T<2>(
581 componentTrackParameters,
583 measurementLocalParameters.head<2>(),
590 calculateWeight_T<3>(componentTrackParameters,
592 measurementLocalParameters.head<3>(),
598 calculateWeight_T<4>(componentTrackParameters,
600 measurementLocalParameters.head<4>(),
606 calculateWeight_T<5>(componentTrackParameters,
608 measurementLocalParameters.head<5>(),
620 determinantRandChi2.elements[determinantRandChi2.numElements] = {
623 ++determinantRandChi2.numElements;
624 if (
result.second < minimumChi2) {
625 minimumChi2 =
result.second;
630 if (determinantRandChi2.numElements != predictedStateSize) {
636 double sumWeights(0.);
637 std::array<double, GSFConstants::maxNumberofStateComponents> fallBackWeights{};
638 auto componentItr = returnMultiComponentState.begin();
639 for (; componentItr != returnMultiComponentState.end();
640 ++componentItr, ++
index) {
642 const double chi2 = determinantRandChi2.elements[
index].chi2 - minimumChi2;
643 const double priorWeight = componentItr->weight;
644 fallBackWeights[
index] = priorWeight;
645 double updatedWeight(0.);
648 if (determinantRandChi2.elements[
index].determinantR > 1
e-20) {
651 sqrt(1. / determinantRandChi2.elements[
index].determinantR) *
654 updatedWeight = 1
e-10;
656 componentItr->weight = updatedWeight;
657 sumWeights += updatedWeight;
659 if (sumWeights > 0.) {
660 const double invertSumWeights = 1. / sumWeights;
662 for (
auto& returnComponent : returnMultiComponentState) {
663 returnComponent.weight *= invertSumWeights;
667 size_t fallbackIndex(0);
668 for (
auto& returnComponent : returnMultiComponentState) {
669 returnComponent.weight = fallBackWeights[fallbackIndex];
673 return returnMultiComponentState;
683 const auto* measuredCov = trackParameters->covariance();
688 for (
int i = 0;
i < 5; ++
i) {
689 if ((*measuredCov)(
i,
i) <= 0.) {
701 std::move(stateBeforeUpdate);
706 const bool rebuildCov = invalidComponent(trackParameters);
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;
716 bigNewCovarianceMatrix);
719 return stateWithInsertedErrors;
731 const Trk::MeasurementBase& measurement,
732 Trk::FitQualityOnSurface& fitQoS)
736 if (stateBeforeUpdate.empty()) {
742 weights(std::move(stateBeforeUpdate), measurement);
744 if (stateWithNewWeights.empty()) {
751 if (stateWithNewWeights.size() > 1 &&
752 std::abs(component.params->parameters()[
Trk::qOverP]) > 0.033333) {
757 bool const updateSuccess = filterStep(*(component.params),
759 measurement.localParameters(),
760 measurement.localCovariance());
761 if (!updateSuccess) {
765 if (invalidComponent(component.params.get())) {
783 std::move(component));
789 if (assembledUpdatedState.empty()) {
797 return assembledUpdatedState;
811 bool rebuildStateWithErrors =
false;
815 rebuildStateWithErrors =
816 rebuildStateWithErrors || invalidComponent(component.params.get());
819 if (rebuildStateWithErrors) {
821 rebuildState(std::move(stateBeforeUpdate));
824 std::move(stateWithInsertedErrors), measurement, fitQoS);
825 if (updatedState.empty()) {
833 std::move(stateBeforeUpdate), measurement, fitQoS);
835 if (updatedState.empty()) {
848 stateFitQuality(componentFitQuality,
849 *component.params.get(),