Skip to content

Commit

Permalink
odom imu mag: define covariance to allow customization
Browse files Browse the repository at this point in the history 
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 <[email protected]>
  • Loading branch information
@hippo5329
hippo5329 committed May 13, 2024
1 parent b694e58 commit 00b586d
Show file tree
Hide file tree
Showing 8 changed files with 106 additions and 29 deletions.
9 changes: 9 additions & 0 deletions config/custom/esp32_config.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
9 changes: 9 additions & 0 deletions config/custom/esp32s2_config.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
9 changes: 9 additions & 0 deletions config/custom/esp32s3_config.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
9 changes: 9 additions & 0 deletions config/custom/pico_config.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
50 changes: 34 additions & 16 deletions firmware/lib/imu/imu_interface.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,16 @@

#include <sensor_msgs/msg/imu.h>

#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:
Expand All @@ -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_;
Expand All @@ -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()
{
Expand All @@ -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_;
}
};
Expand Down
15 changes: 11 additions & 4 deletions firmware/lib/imu/mag_interface.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,11 +18,15 @@

#include <sensor_msgs/msg/magnetic_field.h>

#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()
Expand All @@ -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_;
}
};
Expand Down
24 changes: 16 additions & 8 deletions firmware/lib/odometry/odometry.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Expand All @@ -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)
{
Expand All @@ -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;
}
}
10 changes: 9 additions & 1 deletion firmware/lib/odometry/odometry.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,14 @@
#include <micro_ros_utilities/type_utilities.h>
#include <micro_ros_utilities/string_utilities.h>
#include <nav_msgs/msg/odometry.h>
#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
{
Expand All @@ -36,4 +44,4 @@ class Odometry
float heading_;
};

#endif
#endif

0 comments on commit 00b586d

Please sign in to comment.