comp_filter_estimator.h

#ifndef COMP_FILTER_H
#define COMP_FILTER_H
 
#include <robot_driver/estimators/state_estimator.h>
 
class CompFilterEstimator : public StateEstimator {
 public:
  CompFilterEstimator();
 
  void init(ros::NodeHandle& nh);
 
  void mocapCallBackHelper(const geometry_msgs::PoseStamped::ConstPtr& msg,
                           const Eigen::Vector3d& pos);
 
  bool updateOnce(quad_msgs::RobotState& last_robot_state_msg);
 
 private:
  struct Filter {
    // State-space model
    Eigen::Matrix<double, 2, 2> A;
    Eigen::Matrix<double, 2, 1> B;
    Eigen::Matrix<double, 1, 2> C;
    Eigen::Matrix<double, 1, 1> D;
 
    // Filter states
    std::vector<Eigen::Matrix<double, 2, 1>> x;
 
    // Filter initialization indicator
    bool init;
  };
 
  Filter low_pass_filter;
 
  Filter high_pass_filter;
 
  std::vector<double> high_pass_a_;
  std::vector<double> high_pass_b_;
  std::vector<double> high_pass_c_;
  std::vector<double> high_pass_d_;
 
  std::vector<double> low_pass_a_;
  std::vector<double> low_pass_b_;
  std::vector<double> low_pass_c_;
  std::vector<double> low_pass_d_;
 
  Eigen::Vector3d vel_estimate_;
 
  Eigen::Vector3d mocap_vel_estimate_;
 
  Eigen::Vector3d imu_vel_estimate_;
 
  ros::Time last_mocap_time_;
 
  ros::NodeHandle nh_;
};
#endif  // COMP_FILTER_H