ekf.h

Go to the documentation of this file.

// Copyright (C) 2022 Christian Brommer, Control of Networked Systems, University of Klagenfurt, Austria.
//
// All rights reserved.
//
// This software is licensed under the terms of the BSD-2-Clause-License with
// no commercial use allowed, the full terms of which are made available
// in the LICENSE file. No license in patents is granted.
//
// You can contact the author at <christian.brommer@ieee.org>
 
#ifndef EKF_HPP
#define EKF_HPP
 
#include <Eigen/Dense>
#include <boost/math/distributions/chi_squared.hpp>
#include <iostream>
 
namespace mars
{
class Chi2
{
public:
  Chi2();
 
  Chi2(const int& dof, const double& chi_value);
 
  void set_dof(const int& value);
 
  void set_chi_value(const double& value);
 
  void get_result(Eigen::MatrixXd* const last_res, double* const last_X2) const;
 
  void CalculateUcv();
 
  void ActivateTest(const bool& value);
 
  bool CalculateChi2(const Eigen::MatrixXd& res, const Eigen::MatrixXd& S);
 
  void PrintReport(const std::string& name);
 
  boost::math::chi_squared dist_;  
  int dof_{ 3 };                   
  double chi_value_{ 0.05 };       
  double ucv_;                     
  bool do_test_{ true };          
  bool passed_{ false };           
 
private:
  Eigen::MatrixXd last_res_;  
  double last_X2_;            
};
 
class Ekf
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  Ekf(const Eigen::Ref<const Eigen::MatrixXd>& H, const Eigen::Ref<const Eigen::MatrixXd>& R,
      const Eigen::Ref<const Eigen::MatrixXd>& res, const Eigen::Ref<const Eigen::MatrixXd>& P)
  {
    this->H_ = H;
    this->R_ = R;
    this->res_ = res;
    this->P_ = P;
  }
 
  Eigen::MatrixXd H_;    
  Eigen::MatrixXd R_;    
  Eigen::MatrixXd res_;  
  Eigen::MatrixXd P_;    
  Eigen::MatrixXd S_;    
  Eigen::MatrixXd K_;    
 
  Eigen::MatrixXd CalculateCorrection();
  Eigen::MatrixXd CalculateCorrection(Chi2* chi2);
  Eigen::MatrixXd CalculateCovUpdate();
 
private:
  Eigen::MatrixXd CalculateStateCorrection();
};
}  // namespace mars
 
#endif  // EKF_HPP