TrackFitting/detail/TrackletJoining.cpp

0001 // This file is part of the ACTS project.
0002 //
0003 // Copyright (C) 2016 CERN for the benefit of the ACTS project
0004 //
0005 // This Source Code Form is subject to the terms of the Mozilla Public
0006 // License, v. 2.0. If a copy of the MPL was not distributed with this
0007 // file, You can obtain one at https://mozilla.org/MPL/2.0/.
0008
0009 #include "Acts/TrackFitting/detail/TrackletJoining.hpp"
0010
0011 #include "Acts/EventData/MeasurementHelpers.hpp"
0012 #include "Acts/EventData/TrackParameterHelpers.hpp"
0013 #include "Acts/TrackFitting/KalmanFitterError.hpp"
0014
0015 #include <Eigen/Cholesky>
0016
0017 namespace {
0018
0019 using namespace Acts;
0020
0021 /// Inverse residual covariance and Kalman gain of a measurement state
0022 template <std::size_t N>
0023 struct MeasurementGain {
0024   Matrix<N, eBoundSize> h;
0025   SquareMatrix<N> sInv;
0026   Matrix<eBoundSize, N> k;
0027
0028   template <typename track_state_proxy_t>
0029   explicit MeasurementGain(const track_state_proxy_t& ts)
0030       : h(ts.template projectorSubspaceHelper<N>().projector()) {
0031     const BoundMatrix pp = ts.predictedCovariance();
0032     const SquareMatrix<N> s =
0033         h * pp * h.transpose() + ts.template calibratedCovariance<N>();
0034     sInv = s.inverse();
0035     k = pp * h.transpose() * sInv;
0036   }
0037 };
0038
0039 }  // namespace
0040
0041 namespace Acts::detail {
0042
0043 void setSmoothedWithoutSharing(AnyMutableTrackStateProxy ts, BoundVector mean,
0044                                BoundMatrix cov) {
0045   if (ts.hasSmoothed() &&
0046       ((ts.hasFiltered() && ts.smoothed().data() == ts.filtered().data()) ||
0047        (ts.hasPredicted() && ts.smoothed().data() == ts.predicted().data()))) {
0048     ts.unset(TrackStatePropMask::Smoothed);
0049   }
0050   ts.addComponents(TrackStatePropMask::Smoothed);
0051   ts.smoothed() = mean;
0052   ts.smoothedCovariance() = cov;
0053 }
0054
0055 Result<std::pair<BoundVector, BoundMatrix>> smoothPriorNode(
0056     const AnyConstTrackStateProxy& first, const BoundVector& priorMean,
0057     const BoundMatrix& priorCov, const Logger& logger) {
0058   const BoundMatrix predictedCov = first.predictedCovariance();
0059   if (!predictedCov.allFinite()) {
0060     return KalmanFitterError::SmoothFailed;
0061   }
0062   const Eigen::LDLT<BoundMatrix> ldlt(predictedCov);
0063   if (ldlt.info() != Eigen::Success || !(ldlt.vectorD().array() > 0).all()) {
0064     ACTS_DEBUG("Predicted covariance of state " << first.index()
0065                                                 << " is not positive definite");
0066     return KalmanFitterError::SmoothFailed;
0067   }
0068
0069   // G = P F^T Pp^-1
0070   const BoundMatrix jacobian = first.jacobian();
0071   const BoundMatrix g = ldlt.solve(jacobian * priorCov).transpose();
0072
0073   const BoundVector mean = normalizeBoundParameters(
0074       priorMean +
0075       g * subtractBoundParameters(first.smoothed(), first.predicted()));
0076   const BoundMatrix cov =
0077       priorCov + g * (BoundMatrix(first.smoothedCovariance()) - predictedCov) *
0078                      g.transpose();
0079   if (!mean.allFinite() || !cov.allFinite()) {
0080     ACTS_DEBUG("Prior node estimate is not finite");
0081     return KalmanFitterError::SmoothFailed;
0082   }
0083   return std::pair{mean, BoundMatrix(0.5 * (cov + cov.transpose()))};
0084 }
0085
0086 Result<SharedStateCondition> makeSharedStateCondition(
0087     const AnyConstTrackStateProxy& shared, const BoundVector& newMean,
0088     const BoundMatrix& newCov, const Logger& logger) {
0089   const BoundMatrix smoothedCov = shared.smoothedCovariance();
0090   if (!smoothedCov.allFinite()) {
0091     return KalmanFitterError::SmoothFailed;
0092   }
0093   const Eigen::LDLT<BoundMatrix> ldlt(smoothedCov);
0094   if (ldlt.info() != Eigen::Success || !(ldlt.vectorD().array() > 0).all()) {
0095     ACTS_DEBUG("Shared state covariance is not positive definite");
0096     return KalmanFitterError::SmoothFailed;
0097   }
0098
0099   // Z = Pp Ps^-1, applied to the change of the shared state
0100   const BoundMatrix predictedCov = shared.predictedCovariance();
0101   const BoundMatrix z = ldlt.solve(predictedCov).transpose();
0102   const BoundVector deltaMean =
0103       subtractBoundParameters(newMean, shared.smoothed());
0104   const BoundMatrix deltaCov = newCov - smoothedCov;
0105
0106   SharedStateCondition condition;
0107   condition.u = z * deltaMean;
0108   condition.w = z * deltaCov * z.transpose();
0109   if (!condition.u.allFinite() || !condition.w.allFinite()) {
0110     ACTS_DEBUG("Shared state conditioning terms are not finite");
0111     return KalmanFitterError::SmoothFailed;
0112   }
0113   return condition;
0114 }
0115
0116 BoundMatrix filteredSensitivity(const AnyConstTrackStateProxy& ts,
0117                                 const BoundMatrix& dp) {
0118   if (!ts.typeFlags().isMeasurement()) {
0119     return dp;
0120   }
0121   BoundMatrix df = dp;
0122   visit_measurement(ts.calibratedSize(),
0123                     [&]<std::size_t N>(std::integral_constant<std::size_t, N>) {
0124                       const MeasurementGain<N> gain(ts);
0125                       df = dp - gain.k * (gain.h * dp);
0126                     });
0127   return df;
0128 }
0129
0130 Result<void> conditionTrackState(AnyMutableTrackStateProxy ts,
0131                                  const BoundMatrix& dp, const BoundMatrix& df,
0132                                  BoundMatrix& psiHat,
0133                                  const SharedStateCondition& condition,
0134                                  const Logger& logger) {
0135   const BoundMatrix filteredCov = ts.hasFiltered()
0136                                       ? BoundMatrix(ts.filteredCovariance())
0137                                       : BoundMatrix(ts.predictedCovariance());
0138   // T = Df - Pf Psi^
0139   const BoundMatrix t = df - filteredCov * psiHat;
0140
0141   const BoundVector mean =
0142       normalizeBoundParameters(ts.smoothed() + t * condition.u);
0143   const BoundMatrix cov =
0144       ts.smoothedCovariance() + t * condition.w * t.transpose();
0145   if (!mean.allFinite() || !cov.allFinite()) {
0146     ACTS_DEBUG("Conditioned state " << ts.index() << " is not finite");
0147     return KalmanFitterError::SmoothFailed;
0148   }
0149   setSmoothedWithoutSharing(ts, mean, 0.5 * (cov + cov.transpose()));
0150
0151   // Psi~ = H^T S^-1 H Dp + (I - K H)^T Psi^, then transport to the previous
0152   BoundMatrix psiTilde = psiHat;
0153   if (ts.typeFlags().isMeasurement()) {
0154     visit_measurement(
0155         ts.calibratedSize(),
0156         [&]<std::size_t N>(std::integral_constant<std::size_t, N>) {
0157           const MeasurementGain<N> gain(ts);
0158           psiTilde = gain.h.transpose() * (gain.sInv * (gain.h * dp)) + psiHat -
0159                      gain.h.transpose() * (gain.k.transpose() * psiHat);
0160         });
0161   }
0162   psiHat = ts.jacobian().transpose() * psiTilde;
0163   return Result<void>::success();
0164 }
0165
0166 }  // namespace Acts::detail