Odometry drift caused by acceleration drift removal in relative mode

[rokusottervanger]

The following piece of code (introduced in #753), causes my odometry state to drift due to a nonzero resulting acceleration vector after removing the gravitational acceleration.

robot_localization/src/ros_filter.cpp

Lines 2730 to 2738 in 804729e

	if (!relative) {
	// curAttitude is the true world-frame attitude of the sensor
	rotNorm = trans.getBasis().inverse() * normAcc;
	} else {
	// curAttitude is relative to the initial pose of the sensor.
	// Assumption: IMU sensor is rigidly attached to the base_link
	// (but a static rotation is possible).
	rotNorm = target_frame_trans.getBasis().inverse() * trans.getBasis().inverse() * normAcc;
	}

My r_l configuration is as follows

  frequency: 10.0
  print_diagnostics: true
  base_link_frame: "base_link"
  world_frame: "odom"

  transform_time_offset: 0
  two_d_mode: false
  publish_tf: true
  smooth_lagged_data: true
  history_length: 5

  odom0: encoder_odom
  odom0_nodelay: true
  odom0_config: [false, false, false,   # X,Y,Z,
                false, false, false,  # roll,pitch,yaw,
                true,  true,  true,  # X`,Y`,Z`
                false, false, false,   # roll`,pitch`,yaw`,
                false, false, false]  # X``,Y``,Z``

  imu0: imu/data
  imu0_queue_size: 20
  imu0_remove_gravitational_acceleration: true
  imu0_nodelay: true
  imu0_relative: true
  imu0_config: [false, false, false,  # X,Y,Z,
                true,  true,  true,   # roll,pitch,yaw,
                false, false, false,  # X`,Y`,Z`,
                true,  true,  true,   # roll`,pitch`,yaw`,
                true,  true,  false]   # X``,Y``,Z``

I'm aware that no position data is being fused here, so odom might drift with a random walk, but I'm seeing a significant deterioration (and nonzero mean drift, so not purely random) caused by the referenced code. I'm not sure what the theory behind this piece of code is. Could someone please elaborate? @HaoguangYang maybe?

[HaoguangYang]

Seems like you are using "relative" orientation to remove the gravity. The basis of line 2737 are three assumptions:

1. The [0, 0, 0, 1] orientation of base_link is "upright", with its z-axis pointing up and perpendicular to the ground plane.
2. Your IMU registers orientation [0, 0, 0, 1] when the base_link is "upright", as an origin of orientation (e.g. if an integral-based method is used to obtain roll, pitch, and yaw of the IMU, it should start with the robot's base_link being "upright"). However, your IMU frame may be rotated from base_link.
3. The rotation from base_link to your IMU sensor frame is defined in your TF tree.

I wonder how you configured the gravity removal previously, given that your IMU is mounted at an angle?

Another change to acceleration processing, as you have mentioned in #753 , is the compensation of angular acceleration and centrifugal acceleration to the off-center IMU measurement. This processing assumes your IMU is rigidly attached to the same link (rigid body) as base_link, and the translation from base_link to the IMU sensor frame is also defined in your TF tree.

I would appreciate it if you could share some of your faulty data so that I can help identify the problem.

[rokusottervanger]

Seems like you are using "relative" orientation to remove the gravity.

This is true. Is this not a valid use case?

I wonder how you configured the gravity removal previously, given that your IMU is mounted at an angle?

I used the same configuration in version 2.7.3 without problems.

1. The [0, 0, 0, 1] orientation of base_link is "upright", with its z-axis pointing up and perpendicular to the ground plane.

This is only partly valid. base_link is has its z-axis pointing up through the robot, perpendicular to its footprint. However, if the robot's initial state is at an angle, the odom frame might not be level with the floor. Hence, the gravity vector might not point perfectly down in odom.

2. Your IMU registers orientation [0, 0, 0, 1] when the base_link is "upright", as an origin of orientation (e.g. if an integral-based method is used to obtain roll, pitch, and yaw of the IMU, it should start with the robot's base_link being "upright"). However, your IMU frame may be rotated from base_link.

So you're saying the IMU measurement should be such that after transformation to base_link it would be at [0, 0, 0, 1] when base_link is upright? I guess this is valid as long as the transformation in the tf-tree is perfect. I can't guarantee that, but it's close. However, this seems to me like it is an assumption typical for absolute data fusion, so decidedly not relative, right?

3. The rotation from base_link to your IMU sensor frame is defined in your TF tree.

This is perfectly valid in my case.

Maybe I should elaborate on our reason for setting relative = true. I don't need/want the absolute yaw angle of the IMU represented in the odometry. I do however want to use it to help estimate the (relative) yaw, pitch and roll angles.

Another change to acceleration processing, as you have mentioned in #753 , is the compensation of angular acceleration and centrifugal acceleration to the off-center IMU measurement. This processing assumes your IMU is rigidly attached to the same link (rigid body) as base_link, and the translation from base_link to the IMU sensor frame is also defined in your TF tree.

This assumption seems valid in my case. Also, I'm quite sure this is not causing any problems for me at this point.

I'll see if I can make a minimal bag file that shows the difference between releases 2.7.3 and 2.7.4 and share that this week.