11 #include <analysis/VertexFitting/TreeFitter/KalmanCalculator.h>
13 namespace TreeFitter {
22 m_G(sizeRes, sizeState),
23 m_R(sizeRes, sizeRes),
24 m_Rinverse(sizeRes, sizeRes),
25 m_K(sizeState, sizeRes),
26 m_CGt(sizeState, sizeRes)
28 m_R = Eigen::Matrix < double, -1, -1, 0, 5, 5 >::Zero(m_constrDim, m_constrDim);
33 const Eigen::Matrix < double, -1, 1, 0, 5, 1 > & residuals,
34 const Eigen::Matrix < double, -1, -1, 0, 5, MAX_MATRIX_SIZE > & G,
36 const Eigen::Matrix < double, -1, -1, 0, 5, 5 > * V,
42 Eigen::Matrix < double, -1, -1, 0, MAX_MATRIX_SIZE, MAX_MATRIX_SIZE > C = fitparams.
getCovariance().triangularView<Eigen::Lower>();
44 m_CGt = C.selfadjointView<Eigen::Lower>() * G.transpose();
45 Eigen::Matrix < double, -1, -1, 0, 5, 5 > Rtemp = G *
m_CGt;
46 if (V && (weight) && ((*V).diagonal().array() != 0).all()) {
48 const Eigen::Matrix < double, -1, -1, 0, 5, 5 > weightedV =
49 weight * (*V).selfadjointView<Eigen::Lower>();
51 m_R = Rtemp + weightedV;
54 m_R = Rtemp.triangularView<Eigen::Lower>();
57 Eigen::Matrix < double, -1, -1, 0, 5, 5 > RInvtemp;
58 RInvtemp =
m_R.selfadjointView<Eigen::Lower>();
60 if (!
m_Rinverse.allFinite()) {
return ErrCode(ErrCode::Status::inversionerror); }
63 return ErrCode(ErrCode::Status::success);
68 fitparams.getStateVector() -=
m_K *
m_res;
74 Eigen::Matrix < double, -1, 1, 0, 5, 1 > res_prime =
75 m_res +
m_G * (oldState.getStateVector() - fitparams.getStateVector());
76 fitparams.getStateVector() = oldState.getStateVector() -
m_K * res_prime;
77 m_chisq = res_prime.transpose() *
m_Rinverse.selfadjointView<Eigen::Lower>() * res_prime;
80 TREEFITTER_NO_STACK_WARNING
84 Eigen::Matrix < double, -1, -1, 0, MAX_MATRIX_SIZE, MAX_MATRIX_SIZE > fitCov =
87 Eigen::Matrix < double, -1, -1, 0, MAX_MATRIX_SIZE, MAX_MATRIX_SIZE > GRinvGt =
91 Eigen::Matrix < double, -1, -1, 0, MAX_MATRIX_SIZE, MAX_MATRIX_SIZE > deltaCov =
92 fitCov.selfadjointView<Eigen::Lower>() * GRinvGt * fitCov.selfadjointView<Eigen::Lower>();
94 Eigen::Matrix < double, -1, -1, 0, MAX_MATRIX_SIZE, MAX_MATRIX_SIZE > delta =
97 fitparams.
getCovariance().triangularView<Eigen::Lower>() = delta.triangularView<Eigen::Lower>();
101 TREEFITTER_RESTORE_WARNINGS