src/TrackFitting/MbfSmoother.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/MbfSmoother.hpp"
0010
0011 #include "Acts/EventData/TrackParameterHelpers.hpp"
0012
0013 #include <cstdint>
0014
0015 namespace Acts {
0016
0017 void MbfSmoother::calculateSmoothed(InternalTrackState& ts,
0018                                     const BoundMatrix& bigLambdaHat,
0019                                     const BoundVector& smallLambdaHat) const {
0020   ts.smoothedCovariance = ts.filteredCovariance - ts.filteredCovariance *
0021                                                       bigLambdaHat *
0022                                                       ts.filteredCovariance;
0023   ts.smoothed = ts.filtered - ts.filteredCovariance * smallLambdaHat;
0024   // Normalize phi and theta
0025   ts.smoothed = normalizeBoundParameters(ts.smoothed);
0026 }
0027
0028 void MbfSmoother::visitNonMeasurement(const InternalTrackState& ts,
0029                                       BoundMatrix& bigLambdaHat,
0030                                       BoundVector& smallLambdaHat) const {
0031   const InternalTrackState::Jacobian F = ts.jacobian;
0032
0033   bigLambdaHat = F.transpose() * bigLambdaHat * F;
0034   smallLambdaHat = F.transpose() * smallLambdaHat;
0035 }
0036
0037 void MbfSmoother::visitMeasurement(const InternalTrackState& ts,
0038                                    BoundMatrix& bigLambdaHat,
0039                                    BoundVector& smallLambdaHat) const {
0040   assert(ts.measurement.has_value());
0041
0042   const InternalTrackState::Measurement& measurement = ts.measurement.value();
0043   const InternalTrackState::Jacobian F = ts.jacobian;
0044
0045   visit_measurement(measurement.calibratedSize, [&](auto N) -> void {
0046     constexpr std::size_t kMeasurementSize = decltype(N)::value;
0047     std::span<const std::uint8_t, kMeasurementSize> validSubspaceIndices(
0048         measurement.projector.begin(),
0049         measurement.projector.begin() + kMeasurementSize);
0050     FixedBoundSubspaceHelper<kMeasurementSize> subspaceHelper(
0051         validSubspaceIndices);
0052
0053     using ProjectorMatrix = Eigen::Matrix<double, kMeasurementSize, eBoundSize>;
0054     using CovarianceMatrix =
0055         Eigen::Matrix<double, kMeasurementSize, kMeasurementSize>;
0056     using KalmanGainMatrix =
0057         Eigen::Matrix<double, eBoundSize, kMeasurementSize>;
0058
0059     typename TrackStateTraits<kMeasurementSize, true>::Calibrated calibrated{
0060         measurement.calibrated};
0061     typename TrackStateTraits<kMeasurementSize, true>::CalibratedCovariance
0062         calibratedCovariance{measurement.calibratedCovariance};
0063
0064     // Projector matrix
0065     const ProjectorMatrix H = subspaceHelper.projector();
0066
0067     // Residual covariance
0068     const CovarianceMatrix S =
0069         (H * ts.predictedCovariance * H.transpose() + calibratedCovariance);
0070     // TODO Sinv could be cached by the filter step
0071     const CovarianceMatrix SInv = S.inverse();
0072
0073     // Kalman gain
0074     // TODO K could be cached by the filter step
0075     const KalmanGainMatrix K = (ts.predictedCovariance * H.transpose() * SInv);
0076
0077     const Acts::BoundMatrix CHat = (Acts::BoundMatrix::Identity() - K * H);
0078     const Eigen::Matrix<double, kMeasurementSize, 1> y =
0079         (calibrated - H * ts.predicted);
0080
0081     const Acts::BoundMatrix bigLambdaTilde =
0082         (H.transpose() * SInv * H + CHat.transpose() * bigLambdaHat * CHat);
0083     const Eigen::Matrix<double, eBoundSize, 1> smallLambdaTilde =
0084         (-H.transpose() * SInv * y + CHat.transpose() * smallLambdaHat);
0085
0086     bigLambdaHat = F.transpose() * bigLambdaTilde * F;
0087     smallLambdaHat = F.transpose() * smallLambdaTilde;
0088   });
0089 }
0090
0091 }  // namespace Acts