TrackFitting/detail/MbfSmootherImpl.hpp

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 #pragma once
0010
0011 #include "Acts/EventData/AnyTrackStateProxy.hpp"
0012 #include "Acts/TrackFitting/MbfSmoother.hpp"
0013
0014 namespace Acts {
0015
0016 template <std::size_t N>
0017 void MbfSmoother::visitMeasurementImpl(const AnyConstTrackStateProxy& ts,
0018                                        BoundMatrix& bigLambdaHat,
0019                                        BoundVector& smallLambdaHat) const {
0020   const auto F = ts.jacobian();
0021
0022   const auto subspaceHelper = ts.projectorSubspaceHelper<N>();
0023
0024   using ProjectorMatrix = Eigen::Matrix<double, N, eBoundSize>;
0025   using CovarianceMatrix = Eigen::Matrix<double, N, N>;
0026   using KalmanGainMatrix = Eigen::Matrix<double, eBoundSize, N>;
0027
0028   typename TrackStateTraits<N, true>::Calibrated calibrated{ts.calibrated<N>()};
0029   typename TrackStateTraits<N, true>::CalibratedCovariance calibratedCovariance{
0030       ts.calibratedCovariance<N>()};
0031
0032   // Projector matrix
0033   const ProjectorMatrix H = subspaceHelper.projector();
0034
0035   // Predicted parameter covariance
0036   const auto predictedCovariance = ts.predictedCovariance();
0037
0038   // Residual covariance
0039   const CovarianceMatrix S =
0040       (H * predictedCovariance * H.transpose() + calibratedCovariance);
0041   // TODO Sinv could be cached by the filter step
0042   const CovarianceMatrix SInv = S.inverse();
0043
0044   // Kalman gain
0045   // TODO K could be cached by the filter step
0046   const KalmanGainMatrix K = (predictedCovariance * H.transpose() * SInv);
0047
0048   const Acts::BoundMatrix CHat = (Acts::BoundMatrix::Identity() - K * H);
0049   const Eigen::Matrix<double, N, 1> y = (calibrated - H * ts.predicted());
0050
0051   const Acts::BoundMatrix bigLambdaTilde =
0052       (H.transpose() * SInv * H + CHat.transpose() * bigLambdaHat * CHat);
0053   const Eigen::Matrix<double, eBoundSize, 1> smallLambdaTilde =
0054       (-H.transpose() * SInv * y + CHat.transpose() * smallLambdaHat);
0055
0056   bigLambdaHat = F.transpose() * bigLambdaTilde * F;
0057   smallLambdaHat = F.transpose() * smallLambdaTilde;
0058 }
0059
0060 }  // namespace Acts