Belle II Software light-2406-ragdoll
test_KalmanCalculator.h
1/**************************************************************************
2 * basf2 (Belle II Analysis Software Framework) *
3 * Author: The Belle II Collaboration *
4 * *
5 * See git log for contributors and copyright holders. *
6 * This file is licensed under LGPL-3.0, see LICENSE.md. *
7 **************************************************************************/
8#pragma once
9
10#include <Eigen/Core>
11#include <gtest/gtest.h>
12
13#include "analysis/VertexFitting/TreeFitter/FitParams.h"
14#include "analysis/VertexFitting/TreeFitter/KalmanCalculator.h"
15
16namespace {
17
19 class TreeFitterKalmanCalculatorTest : public ::testing::Test {
20 protected:
21 };
22
24 TEST_F(TreeFitterKalmanCalculatorTest, Functions)
25 {
27 TreeFitter::FitParams fitParDim6(6);
28
29 fitParDim6.getStateVector() << Eigen::Matrix<double, 6, 1>::Zero(6, 1);
30
31 fitParDim6.getCovariance() = 2 * Eigen::Matrix<double, 6, 6>::Identity(6, 6);
32
33 Eigen::Matrix<double, 3, 6> G;
34
35 // cppcheck-suppress constStatement
36 G << 1, 0, 0, 1, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 1, 0, 0, 1;
37
38 const Eigen::Matrix<double, 3, 6>& c_G = G;
39
40 // The kalman matrix m_K should be this after calculation:
41 // 0.5 0 0
42 // 0 0.5 0
43 // 0 0 0.5
44 // 0.5 0 0
45 // 0 0.5 0
46 // 0 0 0.5
47 //
48 Eigen::Matrix<double, 3, 1> residuals;
49 // cppcheck-suppress constStatement
50 residuals << .1, .2, .3;
51 const Eigen::Matrix<double, 3, 1>& c_r = residuals;
52
53 kalman.calculateGainMatrix(c_r, c_G, fitParDim6, nullptr, 0);
54
55 kalman.updateState(fitParDim6);
56
57 Eigen::Matrix<double, 6, 1> expectedUpdatedFitpars;
58 // cppcheck-suppress constStatement
59 expectedUpdatedFitpars << -0.05, -0.1, -0.15, -0.05, -0.1, -0.15;
60
61 EXPECT_TRUE(expectedUpdatedFitpars.isApprox(fitParDim6.getStateVector().segment(0, 6))) << "fitpar update failed.";
62
63 Eigen::Matrix<double, 6, 6> expectedUpdatedCov = Eigen::Matrix<double, 6, 6>::Identity(6, 6);
64 // cppcheck-suppress constStatement
65 expectedUpdatedCov.diagonal < -3 > () << -1, -1, -1;
66 // cppcheck-suppress constStatement
67 expectedUpdatedCov.diagonal<3>() << -1, -1, -1;
68
69 kalman.updateCovariance(fitParDim6);
70 Eigen::Matrix<double, 6, 6> updatedCov = fitParDim6.getCovariance().selfadjointView<Eigen::Lower>();
71
72
73 EXPECT_TRUE(updatedCov.isApprox(expectedUpdatedCov)) << "covariance update failed.";
74
75 }
76
77} // namespace
Class to store and manage fitparams (statevector)
Definition: FitParams.h:20
does the calculation of the gain matrix, updates the cov and fitpars