KalmanCalculator.h

/**************************************************************************
 * basf2 (Belle II Analysis Software Framework)                           *
 * Author: The Belle II Collaboration                                     *
 * External Contributor: Wouter Hulsbergen                                *
 *                                                                        *
 * See git log for contributors and copyright holders.                    *
 * This file is licensed under LGPL-3.0, see LICENSE.md.                  *
 **************************************************************************/
#pragma once
 
#include <analysis/VertexFitting/TreeFitter/EigenStackConfig.h>
 
#include <analysis/VertexFitting/TreeFitter/FitParams.h>
#include <analysis/VertexFitting/TreeFitter/ErrCode.h>
 
// inverse() needs this, in the other classes we get away with just Eigen/Core
#include <Eigen/Dense>
 
 
namespace TreeFitter {

  class KalmanCalculator {
  public:

    KalmanCalculator(
      int sizeRes,
      int sizeState
    );

    void updateState(FitParams& fitparams);

    void updateState(FitParams& fitparams, FitParams& oldState);

    void updateCovariance(FitParams& fitparams);

    double getChiSquare() const { return m_chisq;}

    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);

    double getConstraintDim() const { return m_constrDim; }
 
  private:
    int m_constrDim;

    double m_chisq;


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

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

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

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

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

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