Belle II Software light-2406-ragdoll
KalmanCalculator Class Reference

does the calculation of the gain matrix, updates the cov and fitpars More...

#include <KalmanCalculator.h>

Public Member Functions

 KalmanCalculator (int sizeRes, int sizeState)
 constructor

 
void updateState (FitParams &fitparams)
 update statevector
 
void updateState (FitParams &fitparams, FitParams &oldState)
 update statevector
 
void updateCovariance (FitParams &fitparams)
 update the statevectors covariance
 
double getChiSquare () const
 get chi2 of this iteration
 
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
 
double getConstraintDim () const
 get dimension of the constraint
 

Private Attributes

int m_constrDim
 dimension of the constraint

 
double m_chisq
 chi2
 
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 and the largest constraint is the track constraint with 7
 
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, 7 > m_R
 R residual covariance.
 
Eigen::Matrix< double, -1, -1, 0, 7, 7 > m_Rinverse
 R inverse.
 
Eigen::Matrix< double, -1, -1, 0, MAX_MATRIX_SIZE, 7 > m_K
 K kalman gain matrix.
 
Eigen::Matrix< double, -1, -1, 0, MAX_MATRIX_SIZE, 7 > m_CGt
 C times G^t

 

Detailed Description

does the calculation of the gain matrix, updates the cov and fitpars

Definition at line 23 of file KalmanCalculator.h.

Constructor & Destructor Documentation

◆ KalmanCalculator()

KalmanCalculator ( int  sizeRes,
int  sizeState 
)

constructor

Definition at line 14 of file KalmanCalculator.cc.

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 }
Eigen::Matrix< double, -1, -1, 0, 7, 7 > m_Rinverse
R inverse.
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...
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

Member Function Documentation

◆ calculateGainMatrix()

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

Definition at line 31 of file KalmanCalculator.cc.

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 }

◆ getChiSquare()

double getChiSquare ( ) const
inline

get chi2 of this iteration

Definition at line 42 of file KalmanCalculator.h.

42{ return m_chisq;}

◆ getConstraintDim()

double getConstraintDim ( ) const
inline

get dimension of the constraint

Definition at line 53 of file KalmanCalculator.h.

53{ return m_constrDim; }

◆ updateCovariance()

TREEFITTER_NO_STACK_WARNING void updateCovariance ( FitParams fitparams)

update the statevectors covariance

Definition at line 87 of file KalmanCalculator.cc.

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

◆ updateState() [1/2]

void updateState ( FitParams fitparams)

update statevector

Definition at line 65 of file KalmanCalculator.cc.

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 }

◆ updateState() [2/2]

void updateState ( FitParams fitparams,
FitParams oldState 
)

update statevector

Definition at line 73 of file KalmanCalculator.cc.

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 }

Member Data Documentation

◆ m_CGt

Eigen::Matrix< double, -1, -1, 0, MAX_MATRIX_SIZE, 7 > m_CGt
private

C times G^t

Definition at line 89 of file KalmanCalculator.h.

◆ m_chisq

double m_chisq
private

chi2

Definition at line 60 of file KalmanCalculator.h.

◆ m_constrDim

int m_constrDim
private

dimension of the constraint

Definition at line 57 of file KalmanCalculator.h.

◆ m_G

Eigen::Matrix< double, -1, -1, 0, 7, MAX_MATRIX_SIZE > m_G
private

G former H, transforms covraince of {residuals}<->{x,p,E}.

Definition at line 77 of file KalmanCalculator.h.

◆ m_K

Eigen::Matrix< double, -1, -1, 0, MAX_MATRIX_SIZE, 7 > m_K
private

K kalman gain matrix.

Definition at line 86 of file KalmanCalculator.h.

◆ m_R

Eigen::Matrix< double, -1, -1, 0, 7, 7 > m_R
private

R residual covariance.

Definition at line 80 of file KalmanCalculator.h.

◆ m_res

Eigen::Matrix< double, -1, 1, 0, 7, 1 > m_res
private

we know the max sizes of the matrices we assume the tree is smaller than MAX_MATRIX_SIZE parameters and the largest constraint is the track constraint with 7

-> Eigen puts this on the stack

Eigen::Matrix < double, col, row, ColMajor, maxCol, maxRow> -1 = Eigen::Dynamic 0 = Eigen::ColMajor (don't touch) vector holding the residuals

Definition at line 74 of file KalmanCalculator.h.

◆ m_Rinverse

Eigen::Matrix< double, -1, -1, 0, 7, 7 > m_Rinverse
private

R inverse.

Definition at line 83 of file KalmanCalculator.h.


The documentation for this class was generated from the following files: