motion_models.hpp

Go to the documentation of this file.

#pragma once
 
#include <eigen3/Eigen/Dense>
#include <functional>
#include <map>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <typeinfo>
 
struct MotionUpdate {
  double translational_velocity = 0.0; 
  double acceleration_x = 0.0;         
  double acceleration_y = 0.0;         
  double rotational_velocity = 0.0;    
  double steering_angle = 0.0;         
  rclcpp::Time last_update;            
};
 
class MotionModel {
  Eigen::MatrixXf _process_noise_covariance_matrix; 
public:
  explicit MotionModel(const Eigen::MatrixXf &process_noise_covariance_matrix);
 
  virtual Eigen::VectorXf predict_expected_state(const Eigen::VectorXf &expected_state,
                                                 const MotionUpdate &motion_prediction_data,
                                                 const double time_interval) const = 0;
 
  virtual Eigen::MatrixXf get_motion_to_state_matrix(
      const Eigen::VectorXf &expected_state,
      [[maybe_unused]] const MotionUpdate &motion_prediction_data,
      const double time_interval) const = 0;
 
  Eigen::MatrixXf get_process_noise_covariance_matrix(const unsigned int state_size) const;
 
  static Eigen::MatrixXf create_process_noise_covariance_matrix(float process_noise);
 
  virtual ~MotionModel() = default;
};
 
class ImuVelocityModel : public MotionModel {
  Eigen::MatrixXf _process_noise_covariance_matrix;
 
public:
  explicit ImuVelocityModel(const Eigen::MatrixXf &process_noise_covariance_matrix);
 
  Eigen::VectorXf predict_expected_state(const Eigen::VectorXf &expected_state,
                                         const MotionUpdate &motion_prediction_data,
                                         const double time_interval) const override;
  Eigen::MatrixXf get_motion_to_state_matrix(
      const Eigen::VectorXf &expected_state,
      [[maybe_unused]] const MotionUpdate &motion_prediction_data,
      [[maybe_unused]] const double time_interval) const override;
};
 
class NormalVelocityModel : public MotionModel {
  Eigen::MatrixXf _process_noise_covariance_matrix;
 
public:
  explicit NormalVelocityModel(const Eigen::MatrixXf &process_noise_covariance_matrix);
 
  Eigen::VectorXf predict_expected_state(const Eigen::VectorXf &expected_state,
                                         const MotionUpdate &motion_prediction_data,
                                         const double time_interval) const override;
 
  Eigen::MatrixXf get_motion_to_state_matrix(
      const Eigen::VectorXf &expected_state,
      [[maybe_unused]] const MotionUpdate &motion_prediction_data,
      [[maybe_unused]] const double time_interval) const override;
};
 
const std::map<std::string, std::function<std::shared_ptr<MotionModel>(const Eigen::MatrixXf &)>,
               std::less<>>
    motion_model_constructors = {
        {"normal_velocity_model",
         [](const Eigen::MatrixXf &process_noise_covariance_matrix)
             -> std::shared_ptr<MotionModel> {
           return std::make_shared<NormalVelocityModel>(process_noise_covariance_matrix);
         }},
        {"imu_velocity_model",
         [](const Eigen::MatrixXf &process_noise_covariance_matrix)
             -> std::shared_ptr<MotionModel> {
           return std::make_shared<ImuVelocityModel>(process_noise_covariance_matrix);
         }}};