I'm currently trying to estimate orientation from imu data under ros2 humble. I've created a ros2 node "imu_publisher" which get imu data, compute orientation using madwick filter and publish an imu message.
After converting the orientation output of madwick filter (quaternions) into euler angles, I can see rotations up to 180° and down to -180°, which seems correct. However i have another node which listen to imu message and publish a tf where the rotation component is the same as the orientation quaternion of the imu msg.
The problem is when I visualize the tf on rviz, the tf can't have bigger rotation than 90° or lower rotation than -90°. I heard a bit about gimbal lock but i thought quaternions were not affected by this phenomenon :'(
Here is the code of my node :
#include "imu_node.hpp"
IMU_PUBLISHER::IMU_PUBLISHER(char i2c_slave_addr, char i2c_port_num, uint16_t nb_ech_calibration)
: Node("IMU_PUBLISHER"),
mpu6050(MPU6050(i2c_slave_addr,i2c_port_num,nb_ech_calibration)),
madwick(MADWICK((std::abs(mpu6050.gyro_offset_x)+std::abs(mpu6050.gyro_offset_y)+std::abs(mpu6050.gyro_offset_z))/3)),
alpha_(1.0),
theta_(0.0),
phi_(0.0),
psi_(0.0),
prev_theta_(0.0),
prev_phi_(0.0),
prev_psi_(0.0),
//current_orientation(1.0, 0.0, 0.0, 0.0),
file("imu_data_rectified.log")
{
imu_data.header.frame_id="imu_link";
RCLCPP_INFO(this->get_logger(), "\n===============================================================\nIMU CALIBRATION SUCCESS\n===============================================================\n");
prev_time = this->now();
imu_publisher_ = this->create_publisher<sensor_msgs::msg::Imu>("/utils/imu", 1);
imu_timer_ = this->create_wall_timer(100ms, std::bind(&IMU_PUBLISHER::timer_callback, this));
}
IMU_PUBLISHER::~IMU_PUBLISHER(){
file.close();
}
void IMU_PUBLISHER::timer_callback()
{
auto current_time = this->now();
double dt = ((double)(current_time - prev_time).nanoseconds())/(1000000000);
madwick.DELTA_T = (float)dt;
//Récupération des données acceléromètres et gyromètres
//std::cout << "dt : " << dt << std::endl;
std::cout << "DELTA_T : " << madwick.DELTA_T << std::endl;
fflush(stdout);
//IMUData data = mpu6050.get_filtered_data();
/*
double accel_x = data.ax;
double accel_y = data.ay;
double accel_z = data.az;
double gyro_x = -data.gx;
double gyro_y = -data.gy;
double gyro_z = data.gz;
*/
double accel_x = mpu6050.get_accel_x_g();
double accel_y = mpu6050.get_accel_y_g();
double accel_z = mpu6050.get_accel_z_g();
std::cout << std::fixed << std::setprecision(14) << "Accel before normalization : " << accel_x << ", " << accel_y << ", " << accel_z << std::endl;
normalize_accel(accel_x,accel_y,accel_z);
std::cout << std::fixed << std::setprecision(14) << "Accel after normalization : " << accel_x << ", " << accel_y << ", " << accel_z << std::endl;
double gyro_x = mpu6050.get_gyro_x_rad();
double gyro_y = mpu6050.get_gyro_y_rad();
double gyro_z = mpu6050.get_gyro_z_rad();
quaternion q = madwick.filter(accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z);
//quaternion q = madwick.filter(data.ax, data.ay, data.az, data.gx, data.gy, data.gz);
file << accel_x << " " << accel_y << " " << accel_z << " " << gyro_x << " " << gyro_y << " " << gyro_z << std::endl;
imu_data.linear_acceleration.x=accel_x;
imu_data.linear_acceleration.y=accel_y;
imu_data.linear_acceleration.z=accel_z;
imu_data.angular_velocity.x=gyro_x;
imu_data.angular_velocity.y=gyro_y;
imu_data.angular_velocity.z=gyro_z;
imu_data.orientation.x = q.x;
imu_data.orientation.y = q.y;
imu_data.orientation.z = q.z;
imu_data.orientation.w = q.w;
imu_publisher_->publish(imu_data);
prev_time = current_time;
}
Here is the code of the madwick class :
#include "madwick_filter.hpp"
MADWICK::MADWICK(float offset_gyro) : GYRO_MEAN_ERROR(offset_gyro), DELTA_T(0.1), BETA(std::sqrt(3.0f/4.0f) * GYRO_MEAN_ERROR)
{
q_est = quaternion(1.0, 0.0, 0.0, 0.0);
std::cout << "GYRO_MEAN_ERROR : " << GYRO_MEAN_ERROR << std::endl;
std::cout << "BETA : " << BETA << std::endl;
}
MADWICK::~MADWICK()
{
}
quaternion MADWICK::filter(double ax, double ay, double az, double gx, double gy, double gz){
//Variables and constants
quaternion q_est_prev = q_est;
quaternion q_est_dot = {0,0,0,0}; // used as a place holder in equations 42 and 43
//const struct quaternion q_g_ref = {0, 0, 0, 1};// equation (23), reference to field of gravity for gradient descent optimization (not needed because I used eq 25 instead of eq 21
quaternion q_a = {0, ax, ay, az}; // equation (24) raw acceleration values, needs to be normalized
q_a.normalize(); // normalize the acceleration quaternion to be a unit quaternion
double F_g [3] = {0}; // equation(15/21/25) objective function for gravity
double J_g [3][4] = {0}; // jacobian matrix for gravity
quaternion gradient = {0,0,0,0};
/* Integrate angluar velocity to obtain position in angles. */
quaternion q_w; // equation (10), places gyroscope readings in a quaternion
q_w.w = 0; // the real component is zero, which the Madgwick uses to simplfy quat. mult.
q_w.x = gx;
q_w.y = gy;
q_w.z = gz;
q_w = q_w*0.5; // equation (12) dq/dt = (1/2)q*w
q_w = q_est_prev * q_w; // equation (12)
//Compute the objective function for gravity, equation(15), simplified to equation (25) due to the 0's in the acceleration reference quaternion
F_g[0] = 2*(q_est_prev.x * q_est_prev.z - q_est_prev.w * q_est_prev.y) - q_a.x;
F_g[1] = 2*(q_est_prev.w * q_est_prev.x + q_est_prev.y* q_est_prev.z) - q_a.y;
F_g[2] = 2*(0.5 - q_est_prev.x * q_est_prev.x - q_est_prev.y * q_est_prev.y) - q_a.z;
//Compute the Jacobian matrix, equation (26), for gravity
J_g[0][0] = -2 * q_est_prev.y;
J_g[0][1] = 2 * q_est_prev.z;
J_g[0][2] = -2 * q_est_prev.w;
J_g[0][3] = 2 * q_est_prev.x;
J_g[1][0] = 2 * q_est_prev.x;
J_g[1][1] = 2 * q_est_prev.w;
J_g[1][2] = 2 * q_est_prev.z;
J_g[1][3] = 2 * q_est_prev.y;
J_g[2][0] = 0;
J_g[2][1] = -4 * q_est_prev.x;
J_g[2][2] = -4 * q_est_prev.y;
J_g[2][3] = 0;
// now computer the gradient, equation (20), gradient = J_g'*F_g
gradient.w = J_g[0][0] * F_g[0] + J_g[1][0] * F_g[1] + J_g[2][0] * F_g[2];
gradient.x = J_g[0][1] * F_g[0] + J_g[1][1] * F_g[1] + J_g[2][1] * F_g[2];
gradient.y = J_g[0][2] * F_g[0] + J_g[1][2] * F_g[1] + J_g[2][2] * F_g[2];
gradient.z = J_g[0][3] * F_g[0] + J_g[1][3] * F_g[1] + J_g[2][3] * F_g[2];
// Normalize the gradient, equation (44)
gradient.normalize();
/* This is the sensor fusion part of the algorithm.
Combining Gyroscope position angles calculated in the beginning, with the quaternion orienting the accelerometer to gravity created above.
Noticably this new quaternion has not be created yet, I have only calculated the gradient in equation (19).
Madgwick however uses assumptions with the step size and filter gains to optimize the gradient descent,
combining it with the sensor fusion in equations (42-44).
He says the step size has a var alpha, which he assumes to be very large.
This dominates the previous estimation in equation (19) to the point you can ignore it.
Eq. 36 has the filter gain Gamma, which is related to the step size and thus alpha. With alpha being very large,
you can make assumptions to simplify the fusion equatoin of eq.36.
Combining the simplification of the gradient descent equation with the simplification of the fusion equation gets you eq.
41 which can be subdivided into eqs 42-44.
*/
gradient = gradient * BETA; // multiply normalized gradient by beta
q_est_dot = q_w - gradient; // subtract above from q_w, the integrated gyro quaternion
q_est_dot = q_est_dot * DELTA_T;
q_est = q_est_prev + q_est_dot; // Integrate orientation rate to find position
q_est.normalize(); // normalize the orientation of the estimate
//(shown in diagram, plus always use unit quaternions for orientation)
quaternion res = q_est;
to_euler_angles(q_est,roll,pitch,yaw);
std::cout << "orientation (RPY °) : [" << roll << ", " << pitch << ", " << yaw << "]" << std::endl;
fflush(stdout);
return res;
}
quaternion MADWICK::filter(float ax, float ay, float az, float gx, float gy, float gz){
//Variables and constants
quaternion q_est_prev = q_est;
quaternion q_est_dot = {0,0,0,0}; // used as a place holder in equations 42 and 43
//const struct quaternion q_g_ref = {0, 0, 0, 1};// equation (23), reference to field of gravity for gradient descent optimization (not needed because I used eq 25 instead of eq 21
quaternion q_a = {0, ax, ay, az}; // equation (24) raw acceleration values, needs to be normalized
q_a.normalize(); // normalize the acceleration quaternion to be a unit quaternion
float F_g [3] = {0}; // equation(15/21/25) objective function for gravity
float J_g [3][4] = {0}; // jacobian matrix for gravity
quaternion gradient = {0,0,0,0};
/* Integrate angluar velocity to obtain position in angles. */
quaternion q_w; // equation (10), places gyroscope readings in a quaternion
q_w.w = 0; // the real component is zero, which the Madgwick uses to simplfy quat. mult.
q_w.x = gx;
q_w.y = gy;
q_w.z = gz;
q_w = q_w*0.5; // equation (12) dq/dt = (1/2)q*w
q_w = q_est_prev * q_w; // equation (12)
/* NOTE
* Page 10 states equation (40) substitutes equation (13) into it. This seems false, as he actually
* substitutes equation (12), q_se_dot_w, not equation (13), q_se_w.
*
* // quat_scalar(&q_w, deltaT); // equation (13) integrates the angles velocity to position
* // quat_add(&q_w, q_w, q_est_prev); // addition part of equation (13)
*/
/* Compute the gradient by multiplying the jacobian matrix by the objective function. This is equation 20.
The Jacobian matrix, J, is a 3x4 matrix of partial derivatives for each quaternion component in the x y z axes
The objective function, F, is a 3x1 matrix for x y and z.
To multiply these together, the inner dimensions must match, so use J'.
I calculated "by hand" the transpose of J, so I will be using "hard coordinates" to get those values from J.
The matrix multiplcation can also be done hard coded to reduce code.
Note: it is possible to compute the objective function with quaternion multiplcation functions, but it does not take into account the many zeros that cancel terms out and is not optimized like the paper shows
*/
//Compute the objective function for gravity, equation(15), simplified to equation (25) due to the 0's in the acceleration reference quaternion
F_g[0] = 2*(q_est_prev.x * q_est_prev.z - q_est_prev.w * q_est_prev.y) - q_a.x;
F_g[1] = 2*(q_est_prev.w * q_est_prev.x + q_est_prev.y* q_est_prev.z) - q_a.y;
F_g[2] = 2*(0.5 - q_est_prev.x * q_est_prev.x - q_est_prev.y * q_est_prev.y) - q_a.z;
//Compute the Jacobian matrix, equation (26), for gravity
J_g[0][0] = -2 * q_est_prev.y;
J_g[0][1] = 2 * q_est_prev.z;
J_g[0][2] = -2 * q_est_prev.w;
J_g[0][3] = 2 * q_est_prev.x;
J_g[1][0] = 2 * q_est_prev.x;
J_g[1][1] = 2 * q_est_prev.w;
J_g[1][2] = 2 * q_est_prev.z;
J_g[1][3] = 2 * q_est_prev.y;
J_g[2][0] = 0;
J_g[2][1] = -4 * q_est_prev.x;
J_g[2][2] = -4 * q_est_prev.y;
J_g[2][3] = 0;
// now computer the gradient, equation (20), gradient = J_g'*F_g
gradient.w = J_g[0][0] * F_g[0] + J_g[1][0] * F_g[1] + J_g[2][0] * F_g[2];
gradient.x = J_g[0][1] * F_g[0] + J_g[1][1] * F_g[1] + J_g[2][1] * F_g[2];
gradient.y = J_g[0][2] * F_g[0] + J_g[1][2] * F_g[1] + J_g[2][2] * F_g[2];
gradient.z = J_g[0][3] * F_g[0] + J_g[1][3] * F_g[1] + J_g[2][3] * F_g[2];
// Normalize the gradient, equation (44)
gradient.normalize();
gradient = gradient * BETA; // multiply normalized gradient by beta
q_est_dot = q_w - gradient; // subtract above from q_w, the integrated gyro quaternion
q_est_dot = q_est_dot * DELTA_T;
q_est = q_est_prev + q_est_dot; // Integrate orientation rate to find position
q_est.normalize(); // normalize the orientation of the estimate
//(shown in diagram, plus always use unit quaternions for orientation)
quaternion res = q_est;
to_euler_angles(q_est,roll,pitch,yaw);
std::cout << "orientation (RPY °) : [" << roll << ", " << pitch << ", " << yaw << "]" << std::endl;
fflush(stdout);
return res;
}
void MADWICK::to_euler_angles(quaternion q, float& roll, float& pitch, float& yaw) {
// Roll (x-axis rotation)
float sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
float cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
roll = std::atan2(sinr_cosp, cosr_cosp);
// Pitch (y-axis rotation)
float sinp = 2 * (q.w * q.y - q.z * q.x);
if (std::abs(sinp) >= 1)
pitch = std::copysign(M_PI / 2, sinp); // Use 90 degrees if out of range
else
pitch = std::asin(sinp);
// Yaw (z-axis rotation)
float siny_cosp = 2 * (q.w * q.z + q.x * q.y);
float cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
yaw = std::atan2(siny_cosp, cosy_cosp);
// Convert radians to degrees
roll *= (180.0f / M_PI);
pitch *= (180.0f / M_PI);
yaw *= (180.0f / M_PI);
}
Finally, here is the code of the node which is porting the imu msg into a tf :
#include <rclcpp/rclcpp.hpp>
#include "sensor_msgs/msg/imu.hpp"
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
class ImuToTFNode : public rclcpp::Node {
public:
ImuToTFNode() : Node("imu_to_tf_node") {
imu_subscriber_ = this->create_subscription<sensor_msgs::msg::Imu>(
"/utils/imu", 10, std::bind(&ImuToTFNode::imu_callback, this, std::placeholders::_1));
tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
}
private:
void imu_callback(const sensor_msgs::msg::Imu::SharedPtr imu_msg) {
// Créer une transformation entre les cadres "imu" et "map"
geometry_msgs::msg::TransformStamped tf_msg;
tf_msg.header.stamp = this->now();
tf_msg.header.frame_id = "map";
tf_msg.child_frame_id = "imu_test";
// Récupérer l'orientation de l'IMU
/*
tf2::Quaternion orientation;
tf2::fromMsg(imu_msg->orientation, orientation);
*/
tf_msg.transform.rotation.x = imu_msg->orientation.x;
tf_msg.transform.rotation.y = imu_msg->orientation.y;
tf_msg.transform.rotation.z = imu_msg->orientation.z;
tf_msg.transform.rotation.w = imu_msg->orientation.w;
tf_msg.transform.translation.x = 0.0; // Remplacez par votre valeur de translation en X
tf_msg.transform.translation.y = 0.0; // Remplacez par votre valeur de translation en Y
tf_msg.transform.translation.z = 0.25;
// Publier la transformation
tf_broadcaster_->sendTransform(tf_msg);
}
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_subscriber_;
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
};
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<ImuToTFNode>());
rclcpp::shutdown();
return 0;
}
As you can see below, when converted to RPY, the madwick filter quaternion output can have value up to -180 or 180.
[ros2-1] Accel avant normalisation : -0.11059570312500, -0.24145507812500, 0.86450195312500
[ros2-1] Accel apres normalisation : -0.12228954255449, -0.26698533683529, 0.95591008870958
[ros2-1] orientation (RPY °) : [-13.40970325469971, 9.04709053039551, -154.49398803710938]
[ros2-1] DELTA_T : 0.09999807178974
[ros2-1] Accel avant normalisation : -0.13452148437500, -0.23388671875000, 0.86865234375000
[ros2-1] Accel apres normalisation : -0.14789226529545, -0.25713392042294, 0.95499215956662
[ros2-1] orientation (RPY °) : [-12.85645866394043, 9.29825305938721, -154.39842224121094]
[ros2-1] DELTA_T : 0.10001067817211
[ros2-1] Accel avant normalisation : -0.12597656250000, -0.20751953125000, 0.84399414062500
[ros2-1] Accel apres normalisation : -0.14344624761253, -0.23629711331521, 0.96103425968317
[ros2-1] orientation (RPY °) : [-12.73875904083252, 9.16974925994873, -154.41403198242188]
[ros2-1] DELTA_T : 0.10000046342611
[ros2-1] Accel avant normalisation : -0.14404296875000, -0.21655273437500, 0.85522460937500
[ros2-1] Accel apres normalisation : -0.16114037880915, -0.24225680678596, 0.95673685926858
[ros2-1] orientation (RPY °) : [-12.44879531860352, 8.99049091339111, -154.54493713378906]
[ros2-1] DELTA_T : 0.10000327974558
[ros2-1] Accel avant normalisation : -0.11718750000000, -0.21630859375000, 0.87304687500000
[ros2-1] Accel apres normalisation : -0.12919680907614, -0.23847577675305, 0.96251622761727
[ros2-1] orientation (RPY °) : [-12.36839866638184, 9.10458660125732, -154.67362976074219]
[ros2-1] DELTA_T : 0.10000317543745
[ros2-1] Accel avant normalisation : -0.12304687500000, -0.21142578125000, 0.86767578125000
[ros2-1] Accel apres normalisation : -0.13649123048320, -0.23452659840962, 0.96247982765334
[ros2-1] orientation (RPY °) : [-12.38699245452881, 9.06555271148682, -154.70242309570312]
I'm sorry to bother you but i'm stuck on this since a while and can't understand what's going on. If anyone can help me I would be glad :)
