10#include <tracking/trackFindingCDC/numerics/PrecisionMatrixUtil.h>
11#include <tracking/trackFindingCDC/numerics/JacobianMatrixUtil.h>
13#include <tracking/trackFindingCDC/numerics/CovarianceMatrix.h>
14#include <tracking/trackFindingCDC/numerics/PrecisionMatrix.h>
15#include <tracking/trackFindingCDC/numerics/JacobianMatrix.h>
16#include <tracking/trackFindingCDC/numerics/ParameterVector.h>
18#include <tracking/trackFindingCDC/numerics/EigenView.h>
33 namespace TrackFindingCDC {
47 static void transport(
const JacobianMatrix<N, N>& jacobian, CovarianceMatrix<N>& cov)
49 mapToEigen(cov) = mapToEigen(jacobian) * mapToEigen(cov) * mapToEigen(jacobian).transpose();
53 template <
int M,
int N>
54 static CovarianceMatrix<M>
transported(
const JacobianMatrix<M, N>& jacobian,
55 const CovarianceMatrix<N>& cov)
57 CovarianceMatrix<M> result;
58 mapToEigen(result) = mapToEigen(jacobian) * mapToEigen(cov) * mapToEigen(jacobian).transpose();
64 static void scale(
const ParameterVector<N>& scales, CovarianceMatrix<N>& cov)
71 static CovarianceMatrix<N>
scale(
const ParameterVector<N>& scales,
72 const CovarianceMatrix<N>& cov)
79 template <
int N1,
int N2>
80 static CovarianceMatrix < N1 + N2 >
stackBlocks(
const CovarianceMatrix<N1>& block1,
81 const CovarianceMatrix<N2>& block2)
83 CovarianceMatrix < N1 + N2 > result;
84 mapToEigen(result) << mapToEigen(block1), Eigen::Matrix<double, N1, N2>::Zero(),
85 Eigen::Matrix<double, N2, N1>::Zero(), mapToEigen(block2);
90 template <
class ACovarianceMatrix,
int I = 0,
int N = 0>
91 static ACovarianceMatrix
getSub(
const CovarianceMatrix<N>& cov)
93 constexpr const int M =
94 std::remove_reference_t<
decltype(mapToEigen(ACovarianceMatrix()))>::RowsAtCompileTime;
95 return cov.template block<M, M>(I, I);
102 CovarianceMatrix<N> cov;
103 mapToEigen(cov) = mapToEigen(prec).colPivHouseholderQr().inverse();
109 static PrecisionMatrix<N>
toPrecision(
const CovarianceMatrix<N>& cov)
111 PrecisionMatrix<N> prec;
112 mapToEigen(prec) = mapToEigen(cov).colPivHouseholderQr().inverse();
120 CovarianceMatrix<N> cov;
121 mapToEigen(cov) = mapToEigen(prec).inverse();
129 PrecisionMatrix<N> prec;
130 mapToEigen(prec) = mapToEigen(cov).inverse();
146 static double average(
const ParameterVector<N>& par1,
147 const CovarianceMatrix<N>& cov1,
148 const ParameterVector<N>& par2,
149 const CovarianceMatrix<N>& cov2,
150 ParameterVector<N>& par,
151 CovarianceMatrix<N>& cov)
155 PrecisionMatrix<N> precision;
176 template <
int M,
int N1>
177 static double average(
const ParameterVector<N1>& par1,
178 const CovarianceMatrix<N1>& cov1,
179 const JacobianMatrix<N1, M>& ambiguity1,
180 const ParameterVector<M>& par2,
181 const CovarianceMatrix<M>& cov2,
182 ParameterVector<M>& par,
183 CovarianceMatrix<M>& cov)
187 return average(par1, cov1, ambiguity1, par2, cov2, ambiguity2, par, cov);
209 template <
int M,
int N1,
int N2>
210 static double average(
const ParameterVector<N1>& par1,
211 const CovarianceMatrix<N1>& cov1,
212 const JacobianMatrix<N1, M>& ambiguity1,
213 const ParameterVector<N2>& par2,
214 const CovarianceMatrix<N2>& cov2,
215 const JacobianMatrix<N2, M>& ambiguity2,
216 ParameterVector<M>& par,
217 CovarianceMatrix<M>& cov)
219 PrecisionMatrix<N1> precision1 =
toPrecision(cov1);
220 PrecisionMatrix<N2> precision2 =
toPrecision(cov2);
221 PrecisionMatrix<M> precision;
248 template <
int M,
int N1>
250 const CovarianceMatrix<N1>& cov1,
251 const JacobianMatrix<N1, M>& ambiguity1,
252 ParameterVector<M>& par2,
253 CovarianceMatrix<M>& cov2)
256 const auto& ePar1 = mapToEigen(par1);
257 const auto& eCov1 = mapToEigen(cov1);
258 const auto& eAmbiguity1 = mapToEigen(ambiguity1);
260 auto&& ePar2 = mapToEigen(par2);
261 auto&& eCov2 = mapToEigen(cov2);
264 auto residual = ePar1 - eAmbiguity1 * ePar2;
265 auto residualCov = eCov1 + eAmbiguity1 * eCov2 * eAmbiguity1.transpose();
266 auto residualPrecision = residualCov.inverse();
269 Eigen::Matrix<double, 1, 1> chi2 = residual.transpose() * residualPrecision * residual;
272 auto kalmanGain = eCov2 * eAmbiguity1.transpose() * residualPrecision;
273 eCov2 -= kalmanGain * eAmbiguity1 * eCov2;
274 ePar2 += kalmanGain * residual;
279 auto postResidual = ePar1 - eAmbiguity1 * ePar2;
280 auto postResidualCov = eCov1 - eAmbiguity1 * eCov2 * eAmbiguity1.transpose();
281 auto postResidualPrecision = postResidualCov.inverse();
282 Eigen::Matrix<double, 1, 1> postChi2 =
283 postResidual.transpose() * postResidualPrecision * postResidual;
285 assert(std::fabs(postChi2[0] - chi2[0]) < 1e-8);
static PlainMatrix< T, M, N > Identity()
Construct an identity matrix.
Abstract base class for different kinds of events.
Collection of functions related to covariance matrices.
static CovarianceMatrix< N > fromPrecision(const PrecisionMatrix< N > &prec)
Convert precision matrix to covariance matrix - allows for reduced rank.
static void scale(const ParameterVector< N > &scales, CovarianceMatrix< N > &cov)
Scale the covariance inplace by the given factors in each parameter.
static CovarianceMatrix< M > transported(const JacobianMatrix< M, N > &jacobian, const CovarianceMatrix< N > &cov)
Return a copy of the covariance matrix transported with the given jacobian matrix.
static void transport(const JacobianMatrix< N, N > &jacobian, CovarianceMatrix< N > &cov)
Transport the covariance matrix inplace with the given jacobian matrix.
static PrecisionMatrix< N > fullToPrecision(const CovarianceMatrix< N > &cov)
Convert precision matrix to covariance matrix - assumes full rank.
static double kalmanUpdate(const ParameterVector< N1 > &par1, const CovarianceMatrix< N1 > &cov1, const JacobianMatrix< N1, M > &ambiguity1, ParameterVector< M > &par2, CovarianceMatrix< M > &cov2)
Updates a parameter and its covariance by integrating information from parameter vector in a projecte...
static CovarianceMatrix< N > fromFullPrecision(const PrecisionMatrix< N > &prec)
Convert covariance matrix to precision matrix - assumes full rank.
static CovarianceMatrix< N > scale(const ParameterVector< N > &scales, const CovarianceMatrix< N > &cov)
Return a copy of the covariance scaled by the given factors in each parameter.
static PrecisionMatrix< N > toPrecision(const CovarianceMatrix< N > &cov)
Convert covariance matrix to precision matrix - allows for reduced rank.
static CovarianceMatrix< N > identity()
Constructs an identity matrix.
static double average(const ParameterVector< N1 > &par1, const CovarianceMatrix< N1 > &cov1, const JacobianMatrix< N1, M > &ambiguity1, const ParameterVector< N2 > &par2, const CovarianceMatrix< N2 > &cov2, const JacobianMatrix< N2, M > &ambiguity2, ParameterVector< M > &par, CovarianceMatrix< M > &cov)
Averages two parameter vectors from a projected space taking into account their respective covariance...
static CovarianceMatrix< N1+N2 > stackBlocks(const CovarianceMatrix< N1 > &block1, const CovarianceMatrix< N2 > &block2)
Combines two covariance matrices by putting them in two blocks on the diagonal of a larger matrix.
static double average(const ParameterVector< N > &par1, const CovarianceMatrix< N > &cov1, const ParameterVector< N > &par2, const CovarianceMatrix< N > &cov2, ParameterVector< N > &par, CovarianceMatrix< N > &cov)
Averages two parameter vectors taking into account their respective covariances.
static ACovarianceMatrix getSub(const CovarianceMatrix< N > &cov)
Gets a subcovariance from a covariance matrix.
static double average(const ParameterVector< N1 > &par1, const CovarianceMatrix< N1 > &cov1, const JacobianMatrix< N1, M > &ambiguity1, const ParameterVector< M > &par2, const CovarianceMatrix< M > &cov2, ParameterVector< M > &par, CovarianceMatrix< M > &cov)
Averages two parameter vectors, one being from a projected space, taking into account their respectiv...
static JacobianMatrix< N > scale(const ParameterVector< N > &scales)
Calculates the jacobian matrix for a scaling in each parameter.
static double average(const ParameterVector< N > ¶meter1, const PrecisionMatrix< N > &precision1, const ParameterVector< N > ¶meter2, const PrecisionMatrix< N > &precision2, ParameterVector< N > ¶meter, PrecisionMatrix< N > &precision)
Averages two parameter vectors taking into account their respective precision.