src/TrackFitting/GainMatrixSmoother.cpp

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