Belle II Software light-2406-ragdoll
KalmanCalculator.cc
1/**************************************************************************
2 * basf2 (Belle II Analysis Software Framework) *
3 * Author: The Belle II Collaboration *
4 * External Contributor: Wouter Hulsbergen *
5 * *
6 * See git log for contributors and copyright holders. *
7 * This file is licensed under LGPL-3.0, see LICENSE.md. *
8 **************************************************************************/
9
10#include <analysis/VertexFitting/TreeFitter/KalmanCalculator.h>
11
12namespace TreeFitter {
13
15 int sizeRes,
16 int sizeState
17 ) :
18 m_constrDim(sizeRes),
19 m_chisq(1e10),
20 m_res(sizeRes),
21 m_G(sizeRes, sizeState),
22 m_R(sizeRes, sizeRes),
23 m_Rinverse(sizeRes, sizeRes),
24 m_K(sizeState, sizeRes),
25 m_CGt(sizeState, sizeRes)
26 {
27 m_R = Eigen::Matrix < double, -1, -1, 0, 7, 7 >::Zero(m_constrDim, m_constrDim);
28 }
29
30
32 const Eigen::Matrix < double, -1, 1, 0, 7, 1 > & residuals,
33 const Eigen::Matrix < double, -1, -1, 0, 7, MAX_MATRIX_SIZE > & G,
34 const FitParams& fitparams,
35 const Eigen::Matrix < double, -1, -1, 0, 7, 7 > * V,
36 double weight)
37 {
38 m_res = residuals;
39 m_G = G;
40
41 Eigen::Matrix < double, -1, -1, 0, MAX_MATRIX_SIZE, MAX_MATRIX_SIZE > C = fitparams.getCovariance().triangularView<Eigen::Lower>();
42
43 m_CGt = C.selfadjointView<Eigen::Lower>() * G.transpose();
44 Eigen::Matrix < double, -1, -1, 0, 7, 7 > Rtemp = G * m_CGt;
45 if (V && (weight) && ((*V).diagonal().array() != 0).all()) {
46
47 const Eigen::Matrix < double, -1, -1, 0, 7, 7 > weightedV =
48 weight * (*V).selfadjointView<Eigen::Lower>();
49
50 m_R = Rtemp + weightedV;
51
52 } else {
53 m_R = Rtemp.triangularView<Eigen::Lower>();
54 }
55
56 Eigen::Matrix < double, -1, -1, 0, 7, 7 > RInvtemp;
57 RInvtemp = m_R.selfadjointView<Eigen::Lower>();
58 m_Rinverse = RInvtemp.inverse();
59 if (!m_Rinverse.allFinite()) { return ErrCode(ErrCode::Status::inversionerror); }
60
61 m_K = m_CGt * m_Rinverse.selfadjointView<Eigen::Lower>();
62 return ErrCode(ErrCode::Status::success);
63 }
64
66 {
67 double eps = Eigen::NumTraits<double>::epsilon();
68 fitparams.getStateVector() = (fitparams.getStateVector().array() - (m_K * m_res).array()).matrix()
69 + eps * Eigen::MatrixXd::Identity(fitparams.getStateVector().rows(), fitparams.getStateVector().cols());
70 m_chisq = m_res.transpose() * m_Rinverse.selfadjointView<Eigen::Lower>() * m_res;
71 }
72
74 {
75 double eps = Eigen::NumTraits<double>::epsilon();
76 Eigen::Matrix < double, -1, 1, 0, 7, 1 > res_prime =
77 m_res + m_G * ((oldState.getStateVector().array() - fitparams.getStateVector().array()).matrix() +
78 + eps * Eigen::MatrixXd::Identity(fitparams.getStateVector().rows(), fitparams.getStateVector().cols()));
79 fitparams.getStateVector() = (oldState.getStateVector().array() - (m_K * res_prime).array()).matrix()
80 + eps * Eigen::MatrixXd::Identity(oldState.getStateVector().rows(), oldState.getStateVector().cols());
81
82 m_chisq = res_prime.transpose() * m_Rinverse.selfadjointView<Eigen::Lower>() * res_prime;
83 }
84
85 TREEFITTER_NO_STACK_WARNING
86
88 {
89 Eigen::Matrix < double, -1, -1, 0, MAX_MATRIX_SIZE, MAX_MATRIX_SIZE > fitCov =
90 fitparams.getCovariance().triangularView<Eigen::Lower>();
91
92 Eigen::Matrix < double, -1, -1, 0, MAX_MATRIX_SIZE, MAX_MATRIX_SIZE > GRinvGt =
93 m_G.transpose() * m_Rinverse.selfadjointView<Eigen::Lower>() * m_G;
94
95 //fitcov is sym so no transpose needed (not that it would have runtime cost)
96 Eigen::Matrix < double, -1, -1, 0, MAX_MATRIX_SIZE, MAX_MATRIX_SIZE > deltaCov =
97 fitCov.selfadjointView<Eigen::Lower>() * GRinvGt * fitCov.selfadjointView<Eigen::Lower>();
98
99 double eps = Eigen::NumTraits<double>::epsilon();
100 Eigen::Matrix < double, -1, -1, 0, MAX_MATRIX_SIZE, MAX_MATRIX_SIZE > delta =
101 (fitCov.array() - deltaCov.array()).matrix() + eps * Eigen::MatrixXd::Identity(fitCov.rows(), fitCov.cols());
102
103 fitparams.getCovariance().triangularView<Eigen::Lower>() = delta.triangularView<Eigen::Lower>();
104
105 }//end function
106
107 TREEFITTER_RESTORE_WARNINGS
108
109}// end namespace
abstract errorocode be aware that the default is success
Definition: ErrCode.h:14
Class to store and manage fitparams (statevector)
Definition: FitParams.h:20
Eigen::Matrix< double, -1, 1, 0, MAX_MATRIX_SIZE, 1 > & getStateVector()
getter for the fit parameters/statevector
Definition: FitParams.h:65
Eigen::Matrix< double, -1, -1, 0, MAX_MATRIX_SIZE, MAX_MATRIX_SIZE > & getCovariance()
getter for the states covariance
Definition: FitParams.h:53
void updateState(FitParams &fitparams)
update statevector
ErrCode calculateGainMatrix(const Eigen::Matrix< double, -1, 1, 0, 7, 1 > &residuals, const Eigen::Matrix< double, -1, -1, 0, 7, MAX_MATRIX_SIZE > &G, const FitParams &fitparams, const Eigen::Matrix< double, -1, -1, 0, 7, 7 > *V=0, double weight=1)
init the kalman machienery
Eigen::Matrix< double, -1, -1, 0, 7, 7 > m_Rinverse
R inverse.
KalmanCalculator(int sizeRes, int sizeState)
constructor
Eigen::Matrix< double, -1, -1, 0, 7, 7 > m_R
R residual covariance.
Eigen::Matrix< double, -1, -1, 0, 7, MAX_MATRIX_SIZE > m_G
G former H, transforms covraince of {residuals}<->{x,p,E}.
Eigen::Matrix< double, -1, 1, 0, 7, 1 > m_res
we know the max sizes of the matrices we assume the tree is smaller than MAX_MATRIX_SIZE parameters a...
void updateCovariance(FitParams &fitparams)
update the statevectors covariance
Eigen::Matrix< double, -1, -1, 0, MAX_MATRIX_SIZE, 7 > m_K
K kalman gain matrix.
int m_constrDim
dimension of the constraint
Eigen::Matrix< double, -1, -1, 0, MAX_MATRIX_SIZE, 7 > m_CGt
C times G^t