Strange behavior of FilterBase::processMeasurement (imu0_acceleration)

Hello!

In my robot project I want to fuse odom coming from ros2_control DiffDriveController and IMU from oak camera. ROS2 is humble and robot_localization installed using

sudo apt install ros-humble-robot-localization
ros-humble-robot-localization is already the newest version (3.5.1-2jammy.20230525.003924).

Angular velocity / yaw fusion is working correctly -> if robot wheels are in the air -> imu angular zv is zero thus odom angular twist z is ignored, or in case if robot is on the ground its odometry/filtered orientation yaw is updated correctly.

The issue is with imu linear x acceleration and odom linear x velocity. EKF does not listen to imu data at all. Thus when wheels are rotating in the air in +x direction the odometry/filtered still outputs same twist/linear/x as odom from diff drive.

Looking at the debug logs from EKF I noticed that imu ros2 message processing is divided into 2 parts: imu0_twist and imu0_acceleration and time delta for imu0_acceleration is always 0! Is this intended or a bug?

------ RosFilter<T>::integrateMeasurements ------

Integration time is 1685534594.0991454124
11 measurements in queue.
------ FilterBase::processMeasurement (imu0_twist) ------
Filter is already initialized. Carrying out predict/correct loop...
Measurement time is 1685534594064252232, last measurement time is 1685534594063644280, delta is 607952
...
...
...
---------------------- /Ekf::correct ----------------------
------ /FilterBase::processMeasurement (imu0_twist) ------
------ FilterBase::processMeasurement (imu0_acceleration) ------
Filter is already initialized. Carrying out predict/correct loop...
Measurement time is 1685534594064252232, last measurement time is 1685534594064252232, delta is 0
...
...
...

robot_localization_debug.txt

I also add my messages and configs for the reference. IMU has to be in ENU frame, double checked this multiple times. Gravity is also removed from IMU accelerations.

EKF config

ekf_filter_node:
    ros__parameters:
        frequency: 30.0
        sensor_timeout: 0.1
        two_d_mode: true
        publish_tf: true

        map_frame: map              # Defaults to "map" if unspecified
        odom_frame: odom            # Defaults to "odom" if unspecified
        base_link_frame: base_link  # Defaults to "base_link" if unspecified
        world_frame: odom           # Defaults to the value of odom_frame if unspecified

        odom0: diff_cont/odom
        odom0_config: [false, false, false,
                       false, false, false,
                       true, true, false,
                       false, false, true,
                       false, false, false]
        odom0_differential: false
        odom0_relative: false
        odom0_queue_size: 10

        # Further input parameter examples
        imu0: imu/data
        imu0_config: [false, false, false,
                      false, false, false,
                      false, false, false,
                      false, false, true,
                      true, false, false]
        imu0_differential: false
        imu0_relative: false
        imu0_queue_size: 10

        debug: true
        print_diagnostics: true

IMU sample

{
    "header": {
        "stamp": {"sec": 1685532940, "nsec": 313489492},
        "frame_id": "oak_imu_frame",
    },
    "orientation": {
        "x": -0.002411041711994618,
        "y": 0.030930351520896026,
        "z": -0.009934159600002273,
        "w": 0.9994692655133016,
    },
    "orientation_covariance": [0, 0, 0, 0, 0, 0, 0, 0, 0],
    "angular_velocity": {
        "x": -0.003195793367922306,
        "y": -0.004261057823896408,
        "z": -0.014913702383637428,
    },
    "angular_velocity_covariance": [0.0001, 0, 0, 0, 0.0001, 0, 0, 0, 0.0001],
    "linear_acceleration": {
        "x": -0.08811283111572266,
        "y": -0.17912736535072327,
        "z": -0.06977176666259766,
    },
    "linear_acceleration_covariance": [0.0002, 0, 0, 0, 0.0002, 0, 0, 0, 0.0002],
}

Odom from diffdrive

{
    "header": {"stamp": {"sec": 1685532940, "nsec": 315755537}, "frame_id": "odom"},
    "child_frame_id": "base_link",
    "pose": {
        "pose": {
            "position": {"x": -0.4126891905602348, "y": -0.043448705320674415, "z": 0},
            "orientation": {
                "x": 0,
                "y": 0,
                "z": 0.44579659593815324,
                "w": -0.8951342888360131,
            },
        },
        "covariance": [
            0.1,
            0,
            0,
            0,
            0,
            0,
            0,
            0.1,
            0,
            0,
            0,
            0,
            0,
            0,
            0.1,
            0,
            0,
            0,
            0,
            0,
            0,
            0.1,
            0,
            0,
            0,
            0,
            0,
            0,
            0.1,
            0,
            0,
            0,
            0,
            0,
            0,
            0.1,
        ],
    },
    "twist": {
        "twist": {
            "linear": {"x": 2.0816681711721685e-16, "y": 0, "z": 0},
            "angular": {"x": 0, "y": 0, "z": -8.914743943044811e-16},
        },
        "covariance": [
            10,
            0,
            0,
            0,
            0,
            0,
            0,
            10,
            0,
            0,
            0,
            0,
            0,
            0,
            10,
            0,
            0,
            0,
            0,
            0,
            0,
            10,
            0,
            0,
            0,
            0,
            0,
            0,
            10,
            0,
            0,
            0,
            0,
            0,
            0,
            10,
        ],
    },
}

cra-ros-pkg / robot_localization

Strange behavior of FilterBase::processMeasurement (imu0_acceleration) #817