12 #include <tracking/trackFindingCDC/numerics/PrecisionMatrixUtil.h>
13 #include <tracking/trackFindingCDC/numerics/JacobianMatrixUtil.h>
15 #include <tracking/trackFindingCDC/numerics/CovarianceMatrix.h>
16 #include <tracking/trackFindingCDC/numerics/PrecisionMatrix.h>
17 #include <tracking/trackFindingCDC/numerics/JacobianMatrix.h>
18 #include <tracking/trackFindingCDC/numerics/ParameterVector.h>
20 #include <tracking/trackFindingCDC/numerics/EigenView.h>
26 #include <type_traits>
35 namespace TrackFindingCDC {
38 struct CovarianceMatrixUtil {
42 static CovarianceMatrix<N>
identity()
51 mapToEigen(cov) = mapToEigen(jacobian) * mapToEigen(cov) * mapToEigen(jacobian).transpose();
55 template <
int M,
int N>
60 mapToEigen(result) = mapToEigen(jacobian) * mapToEigen(cov) * mapToEigen(jacobian).transpose();
73 static CovarianceMatrix<N>
scale(
const ParameterVector<N>& scales,
81 template <
int N1,
int N2>
86 mapToEigen(result) << mapToEigen(block1), Eigen::Matrix<double, N1, N2>::Zero(),
87 Eigen::Matrix<double, N2, N1>::Zero(), mapToEigen(block2);
92 template <
class ACovarianceMatrix,
int I = 0,
int N = 0>
95 constexpr
const int M =
96 std::remove_reference_t<decltype(mapToEigen(ACovarianceMatrix()))>::RowsAtCompileTime;
97 return cov.template block<M, M>(I, I);
105 mapToEigen(cov) = mapToEigen(prec).colPivHouseholderQr().inverse();
114 mapToEigen(prec) = mapToEigen(cov).colPivHouseholderQr().inverse();
123 mapToEigen(cov) = mapToEigen(prec).inverse();
132 mapToEigen(prec) = mapToEigen(cov).inverse();
178 template <
int M,
int N1>
189 return average(par1, cov1, ambiguity1, par2, cov2, ambiguity2, par, cov);
211 template <
int M,
int N1,
int N2>
250 template <
int M,
int N1>
258 const auto& ePar1 = mapToEigen(par1);
259 const auto& eCov1 = mapToEigen(cov1);
260 const auto& eAmbiguity1 = mapToEigen(ambiguity1);
262 auto&& ePar2 = mapToEigen(par2);
263 auto&& eCov2 = mapToEigen(cov2);
266 auto residual = ePar1 - eAmbiguity1 * ePar2;
267 auto residualCov = eCov1 + eAmbiguity1 * eCov2 * eAmbiguity1.transpose();
268 auto residualPrecision = residualCov.inverse();
271 Eigen::Matrix<double, 1, 1> chi2 = residual.transpose() * residualPrecision * residual;
274 auto kalmanGain = eCov2 * eAmbiguity1.transpose() * residualPrecision;
275 eCov2 -= kalmanGain * eAmbiguity1 * eCov2;
276 ePar2 += kalmanGain * residual;
281 auto postResidual = ePar1 - eAmbiguity1 * ePar2;
282 auto postResidualCov = eCov1 - eAmbiguity1 * eCov2 * eAmbiguity1.transpose();
283 auto postResidualPrecision = postResidualCov.inverse();
284 Eigen::Matrix<double, 1, 1> postChi2 =
285 postResidual.transpose() * postResidualPrecision * postResidual;
287 assert(std::fabs(postChi2[0] - chi2[0]) < 1e-8);