KalmanCalculator.h

/**************************************************************************
 
 * BASF2 (Belle Analysis Framework 2)                                     *
 * Copyright(C) 2013 - Belle II Collaboration                             *
 *                                                                        *
 * Author: The Belle II Collaboration                                     *
 * Contributor: Wouter Hulsbergen, Francesco Tenchini, Jo-Frederik Krohn  *
 *                                                                        *
 * This software is provided "as is" without any warranty.                *
 **************************************************************************/
#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() { return m_chisq;}
 
    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);
 
    double chisq() const { return m_chisq ; }
 
    double getConstraintDim() { return m_constrDim; }
 
  private:
    int m_constrDim;
 
    double m_chisq;
 
    Eigen::Matrix < double, -1, 1, 0, 5, 1 > m_res;
 
    Eigen::Matrix < double, -1, -1, 0, 5, MAX_MATRIX_SIZE > m_G;
 
    Eigen::Matrix < double, -1, -1, 0, 5, 5 > m_R;
 
    Eigen::Matrix < double, -1, -1, 0, 5, 5 > m_Rinverse;
 
    Eigen::Matrix < double, -1, -1, 0, MAX_MATRIX_SIZE, 5 > m_K;
 
    Eigen::Matrix < double, -1, -1, 0, MAX_MATRIX_SIZE, 5 > m_CGt;
 
  };
}