Ask Your Question

robot_localization: IMU mounted on movable frame

asked 2021-02-03 06:43:56 -0500

chaotic-bruno gravatar image


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...


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

  seq: 123
    secs: 1612354873
    nsecs: 236956120
  frame_id: "camera_imu_optical_frame"
  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]
  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]
  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"/>

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?

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted

answered 2021-02-11 09:53:28 -0500

chaotic-bruno gravatar image

This problem was not related to robot_localization:

The driver of the realsense camera publishes IMU messages with wrong timestamps (future dated). robot_localization was using the right IMU messages according to timestamps. So the rotation was just delayed in IMU data.

When correcting the timestamps the result is not optimal - there is still a small latency. Maybe this is now related to my covariance matrix?

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools



Asked: 2021-02-03 06:43:56 -0500

Seen: 65 times

Last updated: Feb 11