Setup frames lidar odometry, gps, IMU

edit

Hello dear community, I am trying to fuse localization data from rtabmap icp odometry (odom->base_footprint), rtabmap(map->base_footprint), gps odometry from navsat_tranform node and imu data out of madgwick filter.

<node pkg="nodelet" type="nodelet" name="imu_manager" args="manager" output="screen" />
<node pkg="nodelet" type="nodelet" name="ImuFilterNodelet" args="load imu_filter_madgwick/ImuFilterNodelet imu_manager" output="screen">
    <!-- in -->
    <remap from="/imu/data_raw" to="/zed_multi/zed2_rear/zed_nodelet_rear/imu/data"/>
    <remap from="/imu/mag" to="/zed_multi/zed2_rear/zed_nodelet_rear/imu/mag"/>
    <!-- out -->
    <remap from="/imu/data" to="$(arg imu_topic)"/>

    <param name="publish_debug_topics" value="true"/>
    <param name="publish_tf" value="false"/>
    <param name="world_frame" value="enu"/>
    <param name="use_magnetic_field_msg" value="false"/>
    <param name="use_mag" value="true"/>
    <param name="fixed_frame" value="odom"/>
</node>

<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node">
    <param name="magnetic_declination_radians" value="0.083776"/> 
    <param name="yaw_offset" value="0"/><!--IMU heading face east-->
    <param name="zero_altitude" value="true"/>
    <param name="frequency" value="30"/>
    <param name="use_odometry_yaw" value="false"/>
    <param name="broadcast_cartesian_transform" value="true"/>
    <param name="publish_filtered_gps" value="true"/>

    <remap from="/odometry/filtered" to="/rtabmap/odom"/>
    <remap from="/gps/fix" to="$(arg gps_topic)" />
    <remap from="/imu/data" to="/imu/data/madgwicked" />
    <remap from="/odometry/gps" to="/odometry/navsat_gps" />
</node>

<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">      
        <param name="frame_id"        type="string" value="base_footprint"/>  
        <param name="odom_frame_id"   type="string" value="odom"/>
        <param name="map_frame_id"   type="string" value="map"/>
(...)
</node>
<node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">         
        <remap from="imu"        to="/imu/data/madgwicked"/>
        <param name="frame_id"        type="string" value="base_footprint"/>  
        <param name="odom_frame_id"   type="string" value="odom"/>
        <param name="map_frame_id"   type="string" value="map"/>
(...)
with wait_for_imu = true

However, I am unable to put the gps and the lidar odometry in the same frame. I end up having a rotation betwwend my gps point and my lidar odometry point. To summarize, I am unable to understand what I am doing wrong, either from specifying the wrong frames and therefore having a wrong tf tree, not specifying a yaw offset but then I do not know which physical value or if I have an issue with my hardware.

I get that I have to have an ENU referenced IMU (which is why I used the madgwick filter along with the magnetometer of the IMU). Then by initialising the robot pose using the IMU the orientation should be equal to 0 0 0 when facing east. However when my robot faces around east I have

Odometry.cpp:317::process() Updated initial pose from xyz=0.000000,0.000000,0.000000 rpy=0.000000,-0.000000,0.000000 to xyz=0.000000,0.000000,0.000000 rpy=0.002946,0.002428,-2.387123 with IMU orientation

in rtabmap (easier to read than a quaternion) which could mean that the IMU is not earth referenced. Then again : software, hardware ... (more)