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