test.cpp

KalmanFilter::KalmanFilter(
    double dt,
    Eigen::MatrixXd processMatrix,
    Eigen::MatrixXd obsMtx,
    Eigen::MatrixXd processNoiseCov,
    Eigen::MatrixXd obsNoiseCov,
    Eigen::MatrixXd estimatErrCov)
    : processMtx(processMatrix), obsMtx(obsMtx), processNoiseCov(processNoiseCov), obsNoiseCov(obsNoiseCov), initEstimatedErrCov(estimatErrCov),
      stateDim(obsMtx.rows()), obsDim(processMatrix.rows()), dt(dt), initialized(false),
      identityMtx(obsDim, obsDim), x_hat(obsDim), x_hat_new(obsDim)
{
  identityMtx.setIdentity();
}