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