ekf to fuse imu and wheel odom

for some reason the fuse_imu param is not read correctly from the vesc.lua file, but setting it to true in the header is fine for now

Author: nathantsoi

CMakeLists.txt

@@ -26,7 +26,7 @@ IF(CMAKE_VERSION VERSION_LESS "3.7.0")
     SET(CMAKE_INCLUDE_CURRENT_DIR ON)
 ENDIF()
 
-SET(CMAKE_CXX_FLAGS "-std=c++11 -march=native -Werror -Wall -g")
+SET(CMAKE_CXX_FLAGS "-std=c++11 -march=native -Werror -Wall -g -Wno-error=class-memaccess")
 
 IF(${CMAKE_BUILD_TYPE} MATCHES "Release")
   MESSAGE(STATUS "Additional Flags for Release mode")
@@ -57,6 +57,7 @@ find_package(cv_bridge REQUIRED)
 find_package(OpenCV REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 find_package(rosidl_default_runtime REQUIRED)
+find_package(Eigen3 REQUIRED)
 
 FIND_PACKAGE(Qt5 COMPONENTS Core Widgets Gui WebSockets OpenGL REQUIRED)
 SET(CMAKE_INCLUDE_CURRENT_DIR ON)
@@ -116,11 +117,14 @@ IF(${CMAKE_BUILD_MODE} MATCHES "Hardware")
                  src/vesc_driver/vesc_driver.cpp
                  src/vesc_driver/vesc_interface.cpp
                  src/vesc_driver/vesc_packet.cpp
-                 src/vesc_driver/vesc_packet_factory.cpp)
-  ament_target_dependencies(vesc_driver rclcpp std_msgs geometry_msgs nav_msgs sensor_msgs amrl_msgs rosidl_default_runtime tf2_ros)
+                 src/vesc_driver/vesc_packet_factory.cpp
+                 src/vesc_driver/ekf_fusion.cpp
+                 ${CMAKE_CURRENT_SOURCE_DIR}/../mpu6050driver/src/mpu6050sensor.cpp)
+  ament_target_dependencies(vesc_driver rclcpp std_msgs geometry_msgs nav_msgs sensor_msgs amrl_msgs rosidl_default_runtime tf2_ros Eigen3)
   target_include_directories(vesc_driver PRIVATE
-    $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_cpp>)
-  target_link_libraries(vesc_driver ${libs} ${UT_AUTOMATA_TYPESUPPORT_LINK})
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_cpp>
+    ${CMAKE_CURRENT_SOURCE_DIR}/../mpu6050driver/include)
+  target_link_libraries(vesc_driver ${libs} ${UT_AUTOMATA_TYPESUPPORT_LINK} i2c)
 
   add_executable(gui
         src/gui/gui_main.cc

config/joystick.lua

@@ -9,7 +9,3 @@ joystick_name="Sony_DualShock_4"
 -- "left": left stick only (horizontal for steering, vertical for drive)
 -- "right": right stick only (horizontal for steering, vertical for drive)
 joystick_mode = "both"
-
-
-
-

config/vesc.lua

@@ -23,3 +23,11 @@ max_deceleration = 6.0; -- m/s^2
 
 joystick_normal_speed = 1.0; -- m/s
 joystick_turbo_speed = 2.0; -- m/s
+
+-- IMU fusion parameters
+fuse_imu = true; -- Set to true to fuse IMU data with odometry using EKF
+i2c_bus_number = 7; -- I2C bus number for MPU6050 sensor
+calibrate_imu = true; -- Calibrate IMU on startup
+imu_gyro_range = 0; -- 0=250, 1=500, 2=1000, 3=2000 deg/s
+imu_accel_range = 0; -- 0=2g, 1=4g, 2=8g, 3=16g
+imu_dlpf_bandwidth = 0; -- 0=260Hz, 1=184Hz, 2=94Hz, 3=44Hz, 4=21Hz, 5=10Hz, 6=5Hz

src/vesc_driver/ekf_fusion.cpp

@@ -0,0 +1,146 @@
+// -*- mode:c++; fill-column: 100; -*-
+
+#include "ekf_fusion.h"
+#include <cmath>
+#include <iostream>
+
+namespace vesc_driver
+{
+
+EKFFusion::EKFFusion(float wheelbase)
+  : wheelbase_(wheelbase)
+{
+  // Initialize state to zero
+  state_.setZero();
+  
+  // Initialize covariance matrix with moderate uncertainty
+  P_.setIdentity();
+  P_(0, 0) = 0.01;  // x position variance
+  P_(1, 1) = 0.01;  // y position variance
+  P_(2, 2) = 0.01;  // theta variance
+  P_(3, 3) = 0.1;   // velocity variance
+  P_(4, 4) = 0.1;   // angular velocity variance
+  
+  // Process noise covariance - tuned for vehicle dynamics
+  Q_.setIdentity();
+  Q_(0, 0) = 0.001;  // x position process noise
+  Q_(1, 1) = 0.001;  // y position process noise
+  Q_(2, 2) = 0.005;  // theta process noise
+  Q_(3, 3) = 0.1;    // velocity process noise
+  Q_(4, 4) = 0.1;    // angular velocity process noise
+  
+  // IMU measurement noise - gyroscope noise
+  R_imu_ = 0.01;  // rad/s variance
+}
+
+void EKFFusion::predict(double dt, float rpm, float steering_angle,
+                        float speed_to_erpm_gain, float speed_to_erpm_offset)
+{
+  if (dt <= 0.0 || dt > 1.0) {
+    // Skip invalid time steps
+    return;
+  }
+  
+  // Convert RPM to linear velocity
+  float lin_vel = (rpm - speed_to_erpm_offset) / speed_to_erpm_gain;
+  
+  // Clamp velocity to zero for minuscule values
+  if (std::fabs(lin_vel) < 0.01) {
+    lin_vel = 0.0;
+  }
+  
+  // Calculate angular velocity from steering angle
+  float rot_vel = 0.0;
+  if (std::fabs(steering_angle) > 1e-6) {
+    float turn_radius = wheelbase_ / std::tan(steering_angle);
+    if (std::fabs(turn_radius) > 1e-6) {
+      rot_vel = lin_vel / turn_radius;
+    }
+  }
+  
+  // Extract current state
+  float x = state_(0);
+  float y = state_(1);
+  float theta = state_(2);
+  
+  // Predict new state using odometry motion model
+  float cos_theta = std::cos(theta);
+  float sin_theta = std::sin(theta);
+  
+  state_(0) = x + lin_vel * dt * cos_theta;  // x
+  state_(1) = y + lin_vel * dt * sin_theta;  // y
+  state_(2) = theta + rot_vel * dt;          // theta
+  state_(3) = lin_vel;                        // v
+  state_(4) = rot_vel;                        // omega
+  
+  // Normalize theta to [-pi, pi]
+  while (state_(2) > M_PI) state_(2) -= 2.0 * M_PI;
+  while (state_(2) < -M_PI) state_(2) += 2.0 * M_PI;
+  
+  // Compute Jacobian of motion model F
+  Eigen::Matrix<float, 5, 5> F = Eigen::Matrix<float, 5, 5>::Identity();
+  F(0, 2) = -lin_vel * dt * sin_theta;  // dx/dtheta
+  F(0, 3) = dt * cos_theta;              // dx/dv
+  F(1, 2) = lin_vel * dt * cos_theta;   // dy/dtheta
+  F(1, 3) = dt * sin_theta;              // dy/dv
+  F(2, 4) = dt;                          // dtheta/domega
+  
+  // Predict covariance: P = F * P * F^T + Q
+  P_ = F * P_ * F.transpose() + Q_;
+}
+
+void EKFFusion::updateIMU(float angular_velocity_z, bool available)
+{
+  if (!available) {
+    return;  // No IMU data available
+  }
+  
+  // Measurement model: z = H * x + v
+  // We're measuring omega (angular velocity) from IMU
+  // H = [0, 0, 0, 0, 1] - selects the omega state
+  
+  Eigen::Matrix<float, 1, 5> H;
+  H << 0, 0, 0, 0, 1;
+  
+  // Innovation (measurement residual)
+  float y = angular_velocity_z - state_(4);
+  
+  // Innovation covariance
+  float S = H * P_ * H.transpose() + R_imu_;
+  
+  // Kalman gain
+  Eigen::Matrix<float, 5, 1> K = P_ * H.transpose() / S;
+  
+  // Update state estimate
+  state_ = state_ + K * y;
+  
+  // Normalize theta
+  while (state_(2) > M_PI) state_(2) -= 2.0 * M_PI;
+  while (state_(2) < -M_PI) state_(2) += 2.0 * M_PI;
+  
+  // Update covariance estimate
+  Eigen::Matrix<float, 5, 5> I = Eigen::Matrix<float, 5, 5>::Identity();
+  P_ = (I - K * H) * P_;
+}
+
+void EKFFusion::getState(float& x, float& y, float& theta, float& v, float& omega) const
+{
+  x = state_(0);
+  y = state_(1);
+  theta = state_(2);
+  v = state_(3);
+  omega = state_(4);
+}
+
+void EKFFusion::reset()
+{
+  state_.setZero();
+  P_.setIdentity();
+  P_(0, 0) = 0.01;
+  P_(1, 1) = 0.01;
+  P_(2, 2) = 0.01;
+  P_(3, 3) = 0.1;
+  P_(4, 4) = 0.1;
+}
+
+} // namespace vesc_driver

src/vesc_driver/ekf_fusion.h

@@ -0,0 +1,85 @@
+// -*- mode:c++; fill-column: 100; -*-
+
+#ifndef VESC_DRIVER_EKF_FUSION_H_
+#define VESC_DRIVER_EKF_FUSION_H_
+
+// Suppress Eigen warnings about class-memaccess in NEON optimizations
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wclass-memaccess"
+#include <Eigen/Dense>
+#pragma GCC diagnostic pop
+
+namespace vesc_driver
+{
+
+/**
+ * @brief Extended Kalman Filter for fusing odometry with IMU data
+ * 
+ * State vector: [x, y, theta, v, omega]
+ * - x, y: position in odom frame
+ * - theta: orientation (yaw)
+ * - v: linear velocity
+ * - omega: angular velocity
+ */
+class EKFFusion
+{
+public:
+  /**
+   * @brief Constructor
+   * @param wheelbase The wheelbase of the vehicle (m)
+   */
+  explicit EKFFusion(float wheelbase);
+
+  /**
+   * @brief Predict step using odometry model
+   * @param dt Time step (seconds)
+   * @param rpm Motor RPM from VESC
+   * @param steering_angle Current steering angle (radians)
+   * @param speed_to_erpm_gain Conversion gain from speed to ERPM
+   * @param speed_to_erpm_offset Conversion offset from speed to ERPM
+   */
+  void predict(double dt, float rpm, float steering_angle,
+               float speed_to_erpm_gain, float speed_to_erpm_offset);
+
+  /**
+   * @brief Update step using IMU angular velocity measurement
+   * @param angular_velocity_z Angular velocity from IMU (rad/s)
+   * @param available Whether IMU data is available
+   */
+  void updateIMU(float angular_velocity_z, bool available);
+
+  /**
+   * @brief Get current state estimate
+   * @param x Position x (m)
+   * @param y Position y (m)
+   * @param theta Orientation (rad)
+   * @param v Linear velocity (m/s)
+   * @param omega Angular velocity (rad/s)
+   */
+  void getState(float& x, float& y, float& theta, float& v, float& omega) const;
+
+  /**
+   * @brief Reset the filter to initial state
+   */
+  void reset();
+
+private:
+  // State vector [x, y, theta, v, omega]
+  Eigen::Matrix<float, 5, 1> state_;
+  
+  // State covariance matrix
+  Eigen::Matrix<float, 5, 5> P_;
+  
+  // Process noise covariance
+  Eigen::Matrix<float, 5, 5> Q_;
+  
+  // IMU measurement noise covariance (for omega)
+  float R_imu_;
+  
+  // Vehicle wheelbase
+  float wheelbase_;
+};
+
+} // namespace vesc_driver
+
+#endif // VESC_DRIVER_EKF_FUSION_H_