src/TrackFitting/GainMatrixSmoother.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/GainMatrixSmoother.hpp"
0010
0011 #include "Acts/Definitions/TrackParametrization.hpp"
0012 #include "Acts/EventData/TrackParameterHelpers.hpp"
0013 #include "Acts/EventData/detail/CovarianceHelper.hpp"
0014 #include "Acts/TrackFitting/KalmanFitterError.hpp"
0015
0016 namespace Acts {
0017
0018 Result<void> GainMatrixSmoother::calculate(AnyMutableTrackStateProxy ts,
0019                                            AnyConstTrackStateProxy prev_ts,
0020                                            const Logger& logger) const {
0021   auto prevPredictedCovariance = prev_ts.predictedCovariance();
0022   auto filteredCovariance = ts.filteredCovariance();
0023   auto prevJacobian = prev_ts.jacobian();
0024
0025   ACTS_VERBOSE("Prev. predicted covariance\n"
0026                << prevPredictedCovariance << "\n, inverse: \n"
0027                << prevPredictedCovariance.inverse());
0028
0029   // Gain smoothing matrix
0030   // NB: The jacobian stored in a state is the jacobian from previous
0031   // state to this state in forward propagation
0032   BoundMatrix G = filteredCovariance * prevJacobian.transpose() *
0033                   prevPredictedCovariance.inverse();
0034
0035   if (G.hasNaN()) {
0036     ACTS_VERBOSE("Gain smoothing matrix G has NaNs");
0037
0038     ACTS_VERBOSE("Filtered covariance:\n" << filteredCovariance);
0039     ACTS_VERBOSE("Jacobian:\n" << prevJacobian);
0040     ACTS_VERBOSE("Predicted covariance:\n" << prevPredictedCovariance);
0041     ACTS_VERBOSE("Inverse of predicted covariance:\n"
0042                  << prevPredictedCovariance.inverse());
0043
0044     ACTS_VERBOSE("Gain smoothing matrix G:\n" << G);
0045
0046     return KalmanFitterError::SmoothFailed;
0047   }
0048
0049   ACTS_VERBOSE("Gain smoothing matrix G:\n" << G);
0050
0051   auto prevPredicted = prev_ts.predicted();
0052   auto prevSmoothed = prev_ts.smoothed();
0053
0054   auto filtered = ts.filtered();
0055   auto smoothed = ts.smoothed();
0056   auto smoothedCovariance = ts.smoothedCovariance();
0057   auto prevSmoothedCovariance = prev_ts.smoothedCovariance();
0058
0059   ACTS_VERBOSE("Calculate smoothed parameters:");
0060   ACTS_VERBOSE("Filtered parameters: " << filtered.transpose());
0061   ACTS_VERBOSE("Prev. smoothed parameters: " << prevSmoothed.transpose());
0062   ACTS_VERBOSE("Prev. predicted parameters: " << prevPredicted.transpose());
0063
0064   // Calculate the smoothed parameters
0065   smoothed =
0066       filtered + G * subtractBoundParameters(prevSmoothed, prevPredicted);
0067   // Normalize phi and theta
0068   smoothed = normalizeBoundParameters(smoothed);
0069
0070   ACTS_VERBOSE("Smoothed parameters are: " << smoothed.transpose());
0071   ACTS_VERBOSE("Calculate smoothed covariance:");
0072   ACTS_VERBOSE("Prev. smoothed covariance:\n" << prevSmoothedCovariance);
0073
0074   // And the smoothed covariance
0075   smoothedCovariance =
0076       filteredCovariance +
0077       G * (prevSmoothedCovariance - prevPredictedCovariance) * G.transpose();
0078
0079   if (doCovCheckAndAttemptFix) {
0080     // Check if the covariance matrix is semi-positive definite.
0081     // If not, make one (could do more) attempt to replace it with the
0082     // nearest semi-positive def matrix,
0083     // but it could still be non semi-positive
0084     BoundMatrix smoothedCov = smoothedCovariance;
0085     if (!detail::CovarianceHelper<BoundMatrix>::validate(smoothedCov)) {
0086       ACTS_DEBUG(
0087           "Smoothed covariance is not positive definite. Could result in "
0088           "negative covariance!");
0089     }
0090     // Reset smoothed covariance
0091     smoothedCovariance = smoothedCov;
0092   }
0093
0094   ACTS_VERBOSE("Smoothed covariance is: \n" << smoothedCovariance);
0095
0096   return Result<void>::success();
0097 }
0098
0099 }  // namespace Acts