robot_pose_ekf : IMU frame and base_footprint rotates too much.

edit

Hello everyone,

I am implementing an IMU (accelerometer, gyroscope and magnetometer) and want to fuse it with my odometry to optimize the odometry I'll then put in AMCL.

I use the robot_pose_ekf package and I put a low covariance on yaw orientation of the IMU to give it a lot of "weight" in the final estimation (given the magnetometer is a precise and absolute sensor) but it seems like when I rotate in place of an angle of 90°, my IMU_frame end up 90° away from my IMU_base frame (that is fixed to base_link frame) but my base_link ends up 270 ° away from my odom frame. As seen in this video : here Note that this is my imu_frame and imu_base frames seen within the odom frame (ixed frame in rviz).

This tf_tree shows you my current configuration : tf_tree. The base_footprint_odom is the one created from pure odom so I can can compare the two.

My Imu msgs are published in the imu_frame :

header: 
  seq: 15771
  stamp: 
    secs: 1661346327
    nsecs: 523344384
  frame_id: "imu_frame"
...

The IMU tf is broadcast as follows :

    transform.setRotation(imu_quaternion);

    ROS_INFO("Publishing tf\n");

    tf_br.sendTransform(tf::StampedTransform(transform, current_time, "imu_base", "imu_frame"));

    imu_out=*imu;
    imu_out.header.frame_id="imu_base";
    imu_out.orientation.w=-imu_out.orientation.w; //needed so it's imu_base->imu_frame and not the opposite
    imu_out.orientation_covariance=boost::assign::list_of(1e-9)(0)(0)
                                                          (0)(1e-9)(0)
                                                          (0)(0)(1e-6);

    imu_out.angular_velocity_covariance=boost::assign::list_of(1e-3)(0)(0)
                                                              (0)(1e-3)(0)
                                                              (0)(0)(1e-3);

    imu_out.linear_acceleration_covariance=boost::assign::list_of(1e-3)(0)(0)
                                                                 (0)(1e-3)(0)
                                                                 (0)(0)(1e-6);                                                     
    imu_pub.publish(imu_out);

My robot_pose_ekf launch file looks like this :

<launch>

<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">  
<param name="output_frame" value="odom"/>   
<param name="base_footprint_frame" value="base_footprint"/>   
<param name="freq" value="30.0"/>   
<param name="sensor_timeout" value="1.0"/>    
<param name="odom_used" value="true"/> 
<param name="imu_used" value="true"/>  
<param name="vo_used" value="false"/>

  <remap from="odom" to="/odom" />   
 <remap from="/imu_data" to="listener_cpp/imu/data_raw" /> </node>

</launch>

If anyone has a solution or a suggestion on how to debug this issue, I'd be glad to hear it. Sincerely yours.

EDIT : My tf tree looks like that:

                                          odom

                                             |

                                         base_footprint

                                             |

                                         base_link

                                             |

                                        imu_base

                                             |

                                        imu_frame

With the dynamic tf imu_base->imu_frame being computed from the orientation of my IMU (with imu_base as my fixed frame). Thomas