From 00b586d3bada5cba737addb88dfec7c21db804e1 Mon Sep 17 00:00:00 2001 From: Thomas Chou Date: Mon, 13 May 2024 07:32:34 -0700 Subject: [PATCH] odom imu mag: define covariance to allow customization The covariance of various sensors messages is important to the navigation stack. This patch allows fine tune the covariance in the configuration files for best estimation of robot's pose. Signed-off-by: Thomas Chou --- config/custom/esp32_config.h | 9 ++++++ config/custom/esp32s2_config.h | 9 ++++++ config/custom/esp32s3_config.h | 9 ++++++ config/custom/pico_config.h | 9 ++++++ firmware/lib/imu/imu_interface.h | 50 ++++++++++++++++++++---------- firmware/lib/imu/mag_interface.h | 15 ++++++--- firmware/lib/odometry/odometry.cpp | 24 +++++++++----- firmware/lib/odometry/odometry.h | 10 +++++- 8 files changed, 106 insertions(+), 29 deletions(-) diff --git a/config/custom/esp32_config.h b/config/custom/esp32_config.h index 0a546b65..deeadda2 100644 --- a/config/custom/esp32_config.h +++ b/config/custom/esp32_config.h @@ -40,6 +40,15 @@ // #define USE_AK09918_MAG // #define USE_QMC5883L_MAG // #define MAG_BIAS { 0, 0, 0 } +// #define IMU_TWEAK {} +// #define MAG_TWEAK {} + +#define ACCEL_COV { 0.01, 0.01, 0.01 } +#define GYRO_COV { 0.001, 0.001, 0.001 } +#define ORI_COV { 0.01, 0.01, 0.01 } +#define MAG_COV { 1e-12, 1e-12, 1e-12 } +#define POSE_COV { 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 } +#define TWIST_COV { 0.001, 0.001, 0.001, 0.003, 0.003, 0.003 } #define K_P 0.6 // P constant #define K_I 0.8 // I constant diff --git a/config/custom/esp32s2_config.h b/config/custom/esp32s2_config.h index 3a82493f..96b79ba8 100644 --- a/config/custom/esp32s2_config.h +++ b/config/custom/esp32s2_config.h @@ -40,6 +40,15 @@ // #define USE_AK09918_MAG // #define USE_QMC5883L_MAG // #define MAG_BIAS { 0, 0, 0 } +// #define IMU_TWEAK {} +// #define MAG_TWEAK {} + +#define ACCEL_COV { 0.01, 0.01, 0.01 } +#define GYRO_COV { 0.001, 0.001, 0.001 } +#define ORI_COV { 0.01, 0.01, 0.01 } +#define MAG_COV { 1e-12, 1e-12, 1e-12 } +#define POSE_COV { 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 } +#define TWIST_COV { 0.001, 0.001, 0.001, 0.003, 0.003, 0.003 } #define K_P 0.6 // P constant #define K_I 0.8 // I constant diff --git a/config/custom/esp32s3_config.h b/config/custom/esp32s3_config.h index 80c5f4bb..7f969569 100644 --- a/config/custom/esp32s3_config.h +++ b/config/custom/esp32s3_config.h @@ -40,6 +40,15 @@ // #define USE_AK09918_MAG // #define USE_QMC5883L_MAG // #define MAG_BIAS { 0, 0, 0 } +// #define IMU_TWEAK {} +// #define MAG_TWEAK {} + +#define ACCEL_COV { 0.01, 0.01, 0.01 } +#define GYRO_COV { 0.001, 0.001, 0.001 } +#define ORI_COV { 0.01, 0.01, 0.01 } +#define MAG_COV { 1e-12, 1e-12, 1e-12 } +#define POSE_COV { 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 } +#define TWIST_COV { 0.001, 0.001, 0.001, 0.003, 0.003, 0.003 } #define K_P 0.6 // P constant #define K_I 0.8 // I constant diff --git a/config/custom/pico_config.h b/config/custom/pico_config.h index 12c5f77a..ea7f5fcb 100644 --- a/config/custom/pico_config.h +++ b/config/custom/pico_config.h @@ -40,6 +40,15 @@ // #define USE_AK09918_MAG // #define USE_QMC5883L_MAG // #define MAG_BIAS { 0, 0, 0 } +// #define IMU_TWEAK {} +// #define MAG_TWEAK {} + +#define ACCEL_COV { 0.01, 0.01, 0.01 } +#define GYRO_COV { 0.001, 0.001, 0.001 } +#define ORI_COV { 0.01, 0.01, 0.01 } +#define MAG_COV { 1e-12, 1e-12, 1e-12 } +#define POSE_COV { 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 } +#define TWIST_COV { 0.001, 0.001, 0.001, 0.003, 0.003, 0.003 } #define K_P 0.6 // P constant #define K_I 0.8 // I constant diff --git a/firmware/lib/imu/imu_interface.h b/firmware/lib/imu/imu_interface.h index ba85fe0c..f21e1216 100644 --- a/firmware/lib/imu/imu_interface.h +++ b/firmware/lib/imu/imu_interface.h @@ -17,6 +17,16 @@ #include +#ifndef ACCEL_COV +#define ACCEL_COV { 0.00001, 0.00001, 0.00001 } +#endif +#ifndef GYRO_COV +#define GYRO_COV { 0.00001, 0.00001, 0.00001 } +#endif +#ifndef ORI_COV +#define ORI_COV { 0.00001, 0.00001, 0.00001 } +#endif + class IMUInterface { protected: @@ -25,8 +35,9 @@ class IMUInterface const float mgauss_to_utesla_ = 0.1; const float utesla_to_tesla_ = 0.000001; - float accel_cov_ = 0.00001; - float gyro_cov_ = 0.00001; + const float accel_cov[3] = ACCEL_COV; + const float gyro_cov[3] = GYRO_COV; + const float ori_cov[3] = ORI_COV; const int sample_size_ = 40; geometry_msgs__msg__Vector3 gyro_cal_; @@ -49,7 +60,7 @@ class IMUInterface gyro_cal_.y = gyro_cal_.y / (float)sample_size_; gyro_cal_.z = gyro_cal_.z / (float)sample_size_; } - + public: IMUInterface() { @@ -73,29 +84,36 @@ class IMUInterface { imu_msg_.angular_velocity = readGyroscope(); #ifndef USE_MPU6050_IMU // mpu6050 already calibrated in driver - imu_msg_.angular_velocity.x -= gyro_cal_.x; - imu_msg_.angular_velocity.y -= gyro_cal_.y; - imu_msg_.angular_velocity.z -= gyro_cal_.z; + imu_msg_.angular_velocity.x -= gyro_cal_.x; + imu_msg_.angular_velocity.y -= gyro_cal_.y; + imu_msg_.angular_velocity.z -= gyro_cal_.z; #endif if(imu_msg_.angular_velocity.x > -0.01 && imu_msg_.angular_velocity.x < 0.01 ) - imu_msg_.angular_velocity.x = 0; - + imu_msg_.angular_velocity.x = 0; + if(imu_msg_.angular_velocity.y > -0.01 && imu_msg_.angular_velocity.y < 0.01 ) imu_msg_.angular_velocity.y = 0; if(imu_msg_.angular_velocity.z > -0.01 && imu_msg_.angular_velocity.z < 0.01 ) imu_msg_.angular_velocity.z = 0; - - imu_msg_.angular_velocity_covariance[0] = gyro_cov_; - imu_msg_.angular_velocity_covariance[4] = gyro_cov_; - imu_msg_.angular_velocity_covariance[8] = gyro_cov_; - + + imu_msg_.angular_velocity_covariance[0] = gyro_cov[0]; + imu_msg_.angular_velocity_covariance[4] = gyro_cov[1]; + imu_msg_.angular_velocity_covariance[8] = gyro_cov[2]; + imu_msg_.linear_acceleration = readAccelerometer(); - imu_msg_.linear_acceleration_covariance[0] = accel_cov_; - imu_msg_.linear_acceleration_covariance[4] = accel_cov_; - imu_msg_.linear_acceleration_covariance[8] = accel_cov_; + imu_msg_.linear_acceleration_covariance[0] = accel_cov[0]; + imu_msg_.linear_acceleration_covariance[4] = accel_cov[1]; + imu_msg_.linear_acceleration_covariance[8] = accel_cov[2]; + imu_msg_.orientation_covariance[0] = ori_cov[0]; + imu_msg_.orientation_covariance[4] = ori_cov[1]; + imu_msg_.orientation_covariance[8] = ori_cov[2]; + +#ifdef IMU_TWEAK + IMU_TWEAK +#endif return imu_msg_; } }; diff --git a/firmware/lib/imu/mag_interface.h b/firmware/lib/imu/mag_interface.h index 2bd91903..e97f9d3b 100644 --- a/firmware/lib/imu/mag_interface.h +++ b/firmware/lib/imu/mag_interface.h @@ -18,11 +18,15 @@ #include +#ifndef MAG_COV +#define MAG_COV { 0.00001, 0.00001, 0.00001 } +#endif + class MAGInterface { protected: sensor_msgs__msg__MagneticField mag_msg_; - float mag_cov_ = 0.00001; + const float mag_cov[3] = MAG_COV; public: MAGInterface() @@ -42,10 +46,13 @@ class MAGInterface sensor_msgs__msg__MagneticField getData() { mag_msg_.magnetic_field = readMagnetometer(); - mag_msg_.magnetic_field_covariance[0] = mag_cov_; - mag_msg_.magnetic_field_covariance[4] = mag_cov_; - mag_msg_.magnetic_field_covariance[8] = mag_cov_; + mag_msg_.magnetic_field_covariance[0] = mag_cov[0]; + mag_msg_.magnetic_field_covariance[4] = mag_cov[1]; + mag_msg_.magnetic_field_covariance[8] = mag_cov[2]; +#ifdef MAG_TWEAK + MAG_TWEAK +#endif return mag_msg_; } }; diff --git a/firmware/lib/odometry/odometry.cpp b/firmware/lib/odometry/odometry.cpp index 396833ea..cd6f6e45 100644 --- a/firmware/lib/odometry/odometry.cpp +++ b/firmware/lib/odometry/odometry.cpp @@ -30,6 +30,8 @@ void Odometry::update(float vel_dt, float linear_vel_x, float linear_vel_y, floa float sin_h = sin(heading_); float delta_x = (linear_vel_x * cos_h - linear_vel_y * sin_h) * vel_dt; //m float delta_y = (linear_vel_x * sin_h + linear_vel_y * cos_h) * vel_dt; //m + const float pose_cov[6] = POSE_COV; + const float twist_cov[6] = TWIST_COV; //calculate current position of the robot x_pos_ += delta_x; @@ -52,9 +54,12 @@ void Odometry::update(float vel_dt, float linear_vel_x, float linear_vel_y, floa odom_msg_.pose.pose.orientation.z = (double) q[3]; odom_msg_.pose.pose.orientation.w = (double) q[0]; - odom_msg_.pose.covariance[0] = 0.001; - odom_msg_.pose.covariance[7] = 0.001; - odom_msg_.pose.covariance[35] = 0.001; + odom_msg_.pose.covariance[0] = pose_cov[0]; + odom_msg_.pose.covariance[7] = pose_cov[1]; + odom_msg_.pose.covariance[14] = pose_cov[2]; + odom_msg_.pose.covariance[21] = pose_cov[3]; + odom_msg_.pose.covariance[28] = pose_cov[4]; + odom_msg_.pose.covariance[35] = pose_cov[5]; //linear speed from encoders odom_msg_.twist.twist.linear.x = linear_vel_x; @@ -66,15 +71,18 @@ void Odometry::update(float vel_dt, float linear_vel_x, float linear_vel_y, floa odom_msg_.twist.twist.angular.y = 0.0; odom_msg_.twist.twist.angular.z = angular_vel_z; - odom_msg_.twist.covariance[0] = 0.0001; - odom_msg_.twist.covariance[7] = 0.0001; - odom_msg_.twist.covariance[35] = 0.0001; + odom_msg_.twist.covariance[0] = twist_cov[0]; + odom_msg_.twist.covariance[7] = twist_cov[1]; + odom_msg_.twist.covariance[14] = twist_cov[2]; + odom_msg_.twist.covariance[21] = twist_cov[3]; + odom_msg_.twist.covariance[28] = twist_cov[4]; + odom_msg_.twist.covariance[35] = twist_cov[5]; } nav_msgs__msg__Odometry Odometry::getData() { return odom_msg_; -} +} const void Odometry::euler_to_quat(float roll, float pitch, float yaw, float* q) { @@ -89,4 +97,4 @@ const void Odometry::euler_to_quat(float roll, float pitch, float yaw, float* q) q[1] = cy * cp * sr - sy * sp * cr; q[2] = sy * cp * sr + cy * sp * cr; q[3] = sy * cp * cr - cy * sp * sr; -} \ No newline at end of file +} diff --git a/firmware/lib/odometry/odometry.h b/firmware/lib/odometry/odometry.h index bd1d3c3d..ad65d908 100644 --- a/firmware/lib/odometry/odometry.h +++ b/firmware/lib/odometry/odometry.h @@ -19,6 +19,14 @@ #include #include #include +#include "config.h" + +#ifndef POSE_COV +#define POSE_COV { 0.0001, 0.0001, 0, 0, 0, 0.0001 } +#endif +#ifndef TWIST_COV +#define TWIST_COV { 0.00001, 0.00001, 0, 0, 0, 0.00001 } +#endif class Odometry { @@ -36,4 +44,4 @@ class Odometry float heading_; }; -#endif \ No newline at end of file +#endif