Hello!

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

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

IMU angular velocity + odom angular velocity fusion is working correctly -> if robot wheels are in the air -> imu angular ZV is near zero thus odom angular twist Z is ignored. And in case if robot is on the ground its EKF odometry/filtered orientation yaw is updated correctly. So working perfectly as expected.

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

I was thinking that EKF can fuse any measurements from different sensors, like vel and acceleration...?

In addition:

Looking at the debug logs from EKF I noticed that imu ros2 message processing is divided into 2 parts: imu0twist and imu0acceleration and time delta for imu0acceleration is always 0! Have asked in https://github.com/cra-ros-pkg/robotlocalization/issues/817 about it and seems that it is not 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
...
...
...

Attaching my EKF debug log. robotlocalizationdebug.txt

The setup is very basic and common thus it is strange that it does not work. Or maybe I am missing something important? Please help.

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,
        ],
    },
}