A possible invalid assumption regarding frame id of IMU in differential mode

[RonaldEnsing]

I've experienced some problems with the state estimation only when I use (the orientation of) an IMU in differential mode. If I set differential mode to false, everything is fine. This line

robot_localization/src/ros_filter.cpp

Line 2534 in 086bcfc

poseTmp.frame_id_ = (differential ? finalTargetFrame : msg->header.frame_id);

seems suspicious to me. I would expect that it should use the frame_id of the IMU message. If I replace that line with poseTmp.frame_id_ = msg->header.frame_id; everything looks fine. Any idea why the frame id should be set to finalTargetFrame if differential mode is used?

[ayrton04]

Can you give more detail, such as the problems that you are experiencing?

[RonaldEnsing]

The robot localization reports a left turn while in reality it's turning right, and vice versa. This only happens in differential mode. The figures may help to explain better.

Pose of IMU frame (an_device) w.r.t. base_link.

Trajectory as calculated using wheel encoders and IMU with the differential false and only using orientation of the IMU. This trajectory conforms with what happened in the real world.

Trajectory as calculated using wheel encoders and IMU with the differential true and only using orientation of the IMU. This trajectory does not conform with what happened in the real world.

Both views of the trajectory are made with the TopDownOrtho view in rviz with target frame odom, looking 'down' towards the trajectory.

[ayrton04]

I'm wondering if this is related:

https://answers.ros.org/question/329146/in-robot_localization-why-does-differential-mode-not-use-static-transforms/?answer=331479#post-id-331479

Let me poke at this some more and get back to you.

[ayrton04]

Yeah, the more I think about this issue, the more I'm convinced that we should be transforming all pose data into the target frame before we differentiate it.

However, just to be certain, may I see a sample IMU message and your EKF config? Please include the static transform for your IMU. Thanks.

[RonaldEnsing]

Thanks for the 'ROS Answers' link. I was not aware of that question. The question on 'ROS Answers' seems to be related indeed.

On ROS Answers you posted the following:

Note that as soon as we do that transform, both delta1 and v1 are no longer in the original coordinate frame. They are both actually in the base_link frame of the robot itself. This is correct for velocity data.

In the case as written above in this issue, I would think both delta1 and v1 are in my sensor frame (an_device), since the pose of my sensor frame is expressed in a world-fixed frame. I think that transforming all pose data into the target frame before differentiating work as well.

Here is an example IMU message:

header: 
  seq: 12328
  stamp: 
    secs: 1562334206
    nsecs:  12316616
  frame_id: "an_device"
orientation: 
  x: 0.999277923077
  y: -0.0374300261066
  z: -0.00609981180449
  w: 0.00233416507507
orientation_covariance: [1.1356267922984326e-05, 0.0, 0.0, 0.0, 1.136096512856081e-05, 0.0, 0.0, 0.0, 3.9871137225200304e-05]
angular_velocity: 
  x: 0.0157541763037
  y: -0.0669018179178
  z: -0.421086549759
angular_velocity_covariance: [0.2, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.2]
linear_acceleration: 
  x: 0.669848918915
  y: -1.02291309834
  z: 0.453798532486
linear_acceleration_covariance: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]

This is the state estimation node config:

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

This is the static transform:

rosrun tf tf_echo an_device base_link
At time 0.000
- Translation: [-1.234, 0.001, 0.690]
- Rotation: in Quaternion [1.000, -0.000, 0.000, 0.000]
            in RPY (radian) [3.142, -0.000, -0.001]
            in RPY (degree) [180.000, -0.000, -0.046]

[ayrton04]

Yeah, that's an error on my part. In this case (and perhaps in the ROS Answers question), differentiating the pose data is indeed in the sensor frame. It's only when the pose data is the pose of the robot itself that differentiating it results in velocity in the base_link frame.

There's a bit of other surrounding code with differentiating that I need to make sure I don't break if I change it, but I'll definitely fix this. Thanks for the report.

[RonaldEnsing]

Are there any updates on this? What surrounding code do you think may get affected or needs to be tested if we change this to poseTmp.frame_id_ = msg->header.frame_id;?

[ayrton04]

I apologise, Ronald. I just don't get the cycles to work on this package these days. If you change it and submit a PR, then we can see if it passes the tests. I'll review it in more detail then as well.

[RonaldEnsing]

I apologise, Ronald. I just don't get the cycles to work on this package these days. If you change it and submit a PR, then we can see if it passes the tests. I'll review it in more detail then as well.

OK. I've submitted PR #522.