Belle II Software  release-05-01-25
KalmanCalculator.cc
1 /**************************************************************************
2  * BASF2 (Belle Analysis Framework 2) *
3  * Copyright(C) 2013 - Belle II Collaboration *
4  * *
5  * Author: The Belle II Collaboration *
6  * Contributor: Wouter Hulsbergen, Francesco Tenchini, Jo-Frederik Krohn *
7  * *
8  * This software is provided "as is" without any warranty. *
9  **************************************************************************/
10 
11 #include <analysis/VertexFitting/TreeFitter/KalmanCalculator.h>
12 
13 namespace TreeFitter {
14 
16  int sizeRes,
17  int sizeState
18  ) :
19  m_constrDim(sizeRes),
20  m_chisq(1e10),
21  m_res(sizeRes),
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)
27  {
28  m_R = Eigen::Matrix < double, -1, -1, 0, 5, 5 >::Zero(m_constrDim, m_constrDim);
29  }
30 
31 
33  const Eigen::Matrix < double, -1, 1, 0, 5, 1 > & residuals,
34  const Eigen::Matrix < double, -1, -1, 0, 5, MAX_MATRIX_SIZE > & G,
35  const FitParams& fitparams,
36  const Eigen::Matrix < double, -1, -1, 0, 5, 5 > * V,
37  double weight)
38  {
39  m_res = residuals;
40  m_G = G;
41 
42  Eigen::Matrix < double, -1, -1, 0, MAX_MATRIX_SIZE, MAX_MATRIX_SIZE > C = fitparams.getCovariance().triangularView<Eigen::Lower>();
43 
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()) {
47 
48  const Eigen::Matrix < double, -1, -1, 0, 5, 5 > weightedV =
49  weight * (*V).selfadjointView<Eigen::Lower>();
50 
51  m_R = Rtemp + weightedV;
52 
53  } else {
54  m_R = Rtemp.triangularView<Eigen::Lower>();
55  }
56 
57  Eigen::Matrix < double, -1, -1, 0, 5, 5 > RInvtemp;
58  RInvtemp = m_R.selfadjointView<Eigen::Lower>();
59  m_Rinverse = RInvtemp.inverse();
60  if (!m_Rinverse.allFinite()) { return ErrCode(ErrCode::Status::inversionerror); }
61 
62  m_K = m_CGt * m_Rinverse.selfadjointView<Eigen::Lower>();
63  return ErrCode(ErrCode::Status::success);
64  }
65 
66  void KalmanCalculator::updateState(FitParams& fitparams)
67  {
68  fitparams.getStateVector() -= m_K * m_res;
69  m_chisq = m_res.transpose() * m_Rinverse.selfadjointView<Eigen::Lower>() * m_res;
70  }
71 
72  void KalmanCalculator::updateState(FitParams& fitparams, FitParams& oldState)
73  {
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;
78  }
79 
80  TREEFITTER_NO_STACK_WARNING
81 
83  {
84  Eigen::Matrix < double, -1, -1, 0, MAX_MATRIX_SIZE, MAX_MATRIX_SIZE > fitCov =
85  fitparams.getCovariance().triangularView<Eigen::Lower>();
86 
87  Eigen::Matrix < double, -1, -1, 0, MAX_MATRIX_SIZE, MAX_MATRIX_SIZE > GRinvGt =
88  m_G.transpose() * m_Rinverse.selfadjointView<Eigen::Lower>() * m_G;
89 
90  //fitcov is sym so no transpose needed (not that it would have runtime cost)
91  Eigen::Matrix < double, -1, -1, 0, MAX_MATRIX_SIZE, MAX_MATRIX_SIZE > deltaCov =
92  fitCov.selfadjointView<Eigen::Lower>() * GRinvGt * fitCov.selfadjointView<Eigen::Lower>();
93 
94  Eigen::Matrix < double, -1, -1, 0, MAX_MATRIX_SIZE, MAX_MATRIX_SIZE > delta =
95  fitCov - deltaCov;
96 
97  fitparams.getCovariance().triangularView<Eigen::Lower>() = delta.triangularView<Eigen::Lower>();
98 
99  }//end function
100 
101  TREEFITTER_RESTORE_WARNINGS
102 
103 }// end namespace
TreeFitter::KalmanCalculator::m_chisq
double m_chisq
chi2
Definition: KalmanCalculator.h:83
TreeFitter::KalmanCalculator::m_G
Eigen::Matrix< double, -1, -1, 0, 5, MAX_MATRIX_SIZE > m_G
G former H, transforms covraince of {residuals}<->{x,p,E}.
Definition: KalmanCalculator.h:100
TreeFitter::KalmanCalculator::KalmanCalculator
KalmanCalculator(int sizeRes, int sizeState)
constructor
Definition: KalmanCalculator.cc:23
TreeFitter::KalmanCalculator::m_res
Eigen::Matrix< double, -1, 1, 0, 5, 1 > m_res
we know the max sizes of the matrices we assume the tree is smaller than MAX_MATRIX_SIZE parameters a...
Definition: KalmanCalculator.h:97
TreeFitter::FitParams::getCovariance
Eigen::Matrix< double, -1, -1, 0, MAX_MATRIX_SIZE, MAX_MATRIX_SIZE > & getCovariance()
getter for the states covariance
Definition: FitParams.h:62
TreeFitter::ErrCode
abstract errorocode be aware that the default is succes
Definition: ErrCode.h:23
TreeFitter::KalmanCalculator::m_CGt
Eigen::Matrix< double, -1, -1, 0, MAX_MATRIX_SIZE, 5 > m_CGt
C times G^t
Definition: KalmanCalculator.h:112
TreeFitter::FitParams
Class to store and manage fitparams (statevector)
Definition: FitParams.h:29
TreeFitter::KalmanCalculator::m_R
Eigen::Matrix< double, -1, -1, 0, 5, 5 > m_R
R residual covariance.
Definition: KalmanCalculator.h:103
TreeFitter::KalmanCalculator::m_Rinverse
Eigen::Matrix< double, -1, -1, 0, 5, 5 > m_Rinverse
R inverse.
Definition: KalmanCalculator.h:106
TreeFitter::KalmanCalculator::updateState
void updateState(FitParams &fitparams)
update statevector
Definition: KalmanCalculator.cc:74
TreeFitter::KalmanCalculator::updateCovariance
void updateCovariance(FitParams &fitparams)
update the statevectors covariance
Definition: KalmanCalculator.cc:90
TreeFitter::KalmanCalculator::m_K
Eigen::Matrix< double, -1, -1, 0, MAX_MATRIX_SIZE, 5 > m_K
K kalman gain matrix.
Definition: KalmanCalculator.h:109
TreeFitter::KalmanCalculator::calculateGainMatrix
ErrCode calculateGainMatrix(const Eigen::Matrix< double, -1, 1, 0, 5, 1 > &residuals, const Eigen::Matrix< double, -1, -1, 0, 5, MAX_MATRIX_SIZE > &G, const FitParams &fitparams, const Eigen::Matrix< double, -1, -1, 0, 5, 5 > *V=0, double weight=1)
init the kalman machienery
Definition: KalmanCalculator.cc:40