robot_localization: IMU mounted on movable frame

edit

Hey,

I have a humanoid robot and a realsense D455 with an integrated IMU. I wanted to use robot_localization to combine the odometry (vx,vy) and the IMU (v_yaw). To use the RGBD data effectively, I mounted the camera on the head of the robot.

This works ok so far. Now I wrote a script to move the head during mapping. By this moving I increase the captured area during mapping. There is a transform from base_footprint->base_link->...->Head->camera_link->camera...

tf_tree

The IMU message has a the frame "camera_imu_optical_frame" in the header - which I've checked via "rostopic echo ...".

header: 
  seq: 123
  stamp: 
    secs: 1612354873
    nsecs: 236956120
  frame_id: "camera_imu_optical_frame"
orientation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 0.0
orientation_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: 2.09755526157e-05
  y: 0.0052092242986
  z: -3.17811384321e-07
angular_velocity_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
linear_acceleration: 
  x: -0.189400881529
  y: -9.73039150238
  z: -0.918124854565
linear_acceleration_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]

The IMU of the realsense camera is also calibrated.

robot_localization should now publish the odom->base_footprint transform:

<node pkg="robot_localization" type="ekf_localization_node" name="ekf_local_odom" clear_params="true" output="screen">

  <remap from="odometry/filtered" to="odometry/filtered_local"/>

  <param name="frequency" value="80"/>
  <param name="sensor_timeout" value="0.1"/>
  <param name="two_d_mode" value="true"/>

  <param name="map_frame" value="map" />
  <param name="odom_frame" value="odom"/>
  <param name="base_link_frame" value="base_footprint"/>
  <param name="world_frame" value="odom"/>

  <param name="transform_time_offset" value="0.0"/>

  <param name="predict_to_current_time" value="true"/>

  <param name="odom0" value="/naoqi_driver/odom"/>
  <param name="imu0" value="$(arg imu_topic)"/>


  <!-- The order of the values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. -->
  <rosparam param="odom0_config">[false, false, false,
                                  false, false, false,
                                  true, true, false,
                                  false, false, false,
                                  false, false, false]</rosparam>

  <rosparam param="imu0_config">[
                                 false, false, false,
                                 false, false, false,
                                 false, false, false,
                                 false,  false,  true,
                                 false,  false,  false] </rosparam>

  <param name="odom0_differential" value="false"/>
  <param name="imu0_differential" value="false"/>

  <param name="odom0_relative" value="true"/>
  <param name="imu0_relative" value="true"/>

  <param name="imu0_remove_gravitational_acceleration" value="true"/>

  <param name="print_diagnostics" value="true"/>

  <!-- ======== ADVANCED PARAMETERS ======== -->
  <param name="odom0_queue_size" value="5"/>
  <param name="imu0_queue_size" value="50"/>
</node>

My problem is now that the odometry starts spinning as soon as the head starts moving. I thought that robot_localization would calculate the correct values of the IMU from the transform from base_footprint->camera_imu_optical_frame because of the header. I really want to use the rotation of the IMU because the rotation from the odometry is just bad...

Is there a way to compensate the non-static transform of the IMU or should the IMU always be mounted non-movable?