imu.hpp

// Copyright 2022 Jacob Hartzer
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <https://www.gnu.org/licenses/>.
 
#ifndef SENSORS__IMU_HPP_
#define SENSORS__IMU_HPP_
 
#include <eigen3/Eigen/Eigen>
 
#include <memory>
#include <string>
 
#include "ekf/update/imu_updater.hpp"
#include "sensors/imu_message.hpp"
#include "sensors/sensor.hpp"
 
class IMU : public Sensor
{
public:
  typedef struct Parameters : public Sensor::Parameters
  {
    bool is_extrinsic{false};                    
    bool is_intrinsic{false};                    
    Eigen::Vector3d pos_i_in_b {0, 0, 0};        
    Eigen::Quaterniond ang_i_to_b {1, 0, 0, 0};  
    Eigen::Vector3d acc_bias {0, 0, 0};          
    Eigen::Vector3d omg_bias {0, 0, 0};          
    double pos_stability {1e-9};                 
    double ang_stability {1e-9};                 
    double acc_bias_stability {1e-9};            
    double omg_bias_stability {1e-9};            
    double motion_detection_threshold{1.0};      
    double noise_scale_factor{1.0};              
    Eigen::VectorXd variance {{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}};
  } Parameters;
 
  explicit IMU(IMU::Parameters params);
 
  void Callback(std::shared_ptr<ImuMessage> imu_message);
 
private:
  bool m_is_extrinsic;
  bool m_is_intrinsic;
  std::shared_ptr<EKF> m_ekf;
  ImuUpdater m_imu_updater;
};
 
#endif  // SENSORS__IMU_HPP_