Frequency Limitation in robot_localization's EKF Node

Question

I'm using version 2.7.4 of the robot_localization package for ROS Noetic.

I'm currently utilizing two nodes of the package:

• EKF Local Node: Fuses data from an IMU (100Hz) and wheel encoders (4Hz).
• EKF Global Node: Fuses the output of the EKF Local Node with GPS data (4Hz).

Despite these frequencies, the ekf_global node's frequency appears bounded between 60-65 Hz (asking for 100). And the ekf_local node's frequency appears bounded between 80-85 Hz(asking for 100). This restriction remains even when altering the input frequencies. with a lot of "Failed to meet update rate! Took 0.02000X" warnings in the console.

Configuration:

launch file:

        <launch>
            <arg name="namespace" default="fjcruiser" />
            <arg name="base_link_frame" value="$(arg namespace)/base_footprint" />
            <arg name="use_legacy_odom" default="false" />

            <!-- Load conf and parameters from yaml files -->
            <rosparam command="load" file="$(find ugv_odometry)/config/origins.yaml"/>
        <rosparam command="load" file="$(find ugv_odometry)/params/$(arg namespace)/localization.yaml" />

            <!-- Set the origin as a rosparam -->
            <node pkg="ugv_odometry" type="initializer_node" name="initializer_node" output="screen" />
            <!-- GNSS velocity converter-->
            <node name="gnss_velocity_converter" pkg="ugv_odometry" type="gnss_velocity_converter" output="screen" />
            
            <!-- EKF LOCAL-->
            <node pkg="robot_localization" type="ekf_localization_node" name="ekf_local" respawn="false" output="screen">
                <rosparam command="load" file="$(find ugv_odometry)/config/$(arg namespace)/ekf_local.yaml"/>
                <param name="base_link_frame" value="$(arg base_link_frame)"/>
                <remap from="odometry/filtered" to="relative_odom"/>
            </node>

            <!-- EKF Global -->
            <node pkg="robot_localization" type="ekf_localization_node" name="ekf_global" respawn="true" output="screen">
                <rosparam command="load" file="$(find ugv_odometry)/config/common/ekf_global.yaml"/>
            <param name="base_link_frame" value="$(arg base_link_frame)" />
                <param name="odom0" value="/$(arg namespace)/relative_odom" />
            <param name="odom1" value="/$(arg namespace)/navsat/odometry" />
                <remap from="odometry/filtered" to="odom" />
            </node>

            <!-- Navsat Transform Node -->
            <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true">
                <rosparam command="load" file="$(find ugv_odometry)/config/common/navsat_transform.yaml"/>
                <remap from="imu/data" to="imu" />
                <remap from="gps/fix" to="fix" />
                <remap from="odometry/filtered" to="odom" />
                <remap from="odometry/gps" to="navsat/odometry" />
            </node>
         <launch>

ekf_local.yaml

frequency: 100
sensor_timeout: 0.1
two_d_mode: true
world_frame: odom
odom_frame: odom
debug: false
debug_out_file: /home/developer/workspace/rosbag/ekf_local_debug_file
reset_on_time_jump: true
publish_tf: true
dynamic_process_noise_covariance: true
#transform_time_offset: 0.05

# -------------------------------------
# IMU configuration:
# -------------------------------------

imu0: /fjcruiser/imu
imu0_config: [false, false, false,
                false,  false,  false,
                false, false, false,
                false, false, true,
                false, false, false]
imu0_differential: false
imu0_queue_size: 50 
imu0_remove_gravitational_acceleration: true
imu0_nodelay: true
# -------------------------------------
# GNSS Doppler velocity configuration:
# -------------------------------------

odom0: /fjcruiser/converted_gnss_velocity
odom0_config: [false, false, false,
                       false, false, false,
                       true, true, false,
                       false, false, false,
                       false, false, false]
odom0_differential: false
odom0_queue_size: 4
odom0_nodelay: true

# [ADVANCED] This matrix represents the noise we add to the total error after each prediction step.

process_noise_covariance: [0.005, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0.005, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0.006, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0.03,  0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0.03,  0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0.06,  0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0.5,    0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0.5,    0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0.5,   0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0.01,  0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.01,  0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.02,  0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.01,  0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.01,  0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]

# [ADVANCED] This represents the initial value for the state estimate error covariance matrix.

initial_estimate_covariance: [0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    1e-9, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]

ekf_global.yaml

# The frequency of the output
frequency: 50
sensor_timeout: 0.1
two_d_mode: true
debug: false
debug_out_file: /home/developer/workspace/rosbag/ekf_global_debug_file
reset_on_time_jump: true
publish_tf: true
dynamic_process_noise_covariance: true
#transform_time_offset: 0.05

# The frame IDs
map_frame: map
odom_frame: odom
world_frame: map

# Sensor configurations
# Local EKF odometry settings
odom0_config: [false, false, false,
               false, false, false,
               true, true, false,
               false, false, true,
               false, false, false]
odom0_differential: false
odom0_queue_size: 50
odom0_nodelay: true

# GNSS settings
odom1_config: [true, true, false,
                 false, false, false,
                 false, false, false,
                 false, false, false,
                 false, false, false]
odom1_differential: false
odom1_queue_size: 4
odom1_nodelay: true

# [ADVANCED] This matrix represents the noise we add to the total error after each prediction step.

process_noise_covariance: [0.005, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                          0,    0.005, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                          0,    0,    0.006, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                          0,    0,    0,    0.03, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                          0,    0,    0,    0,    0.03, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                          0,    0,    0,    0,    0,    0.01, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0.025, 0,     0,    0,    0,    0,    0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,     0.025, 0,    0,    0,    0,    0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,     0,     0.04, 0,    0,    0,    0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,     0,     0,    0.01, 0,    0,    0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.01, 0,    0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.02, 0,    0,    0,
                          0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.01, 0,    0,
                          0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.01, 0,
                          0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]

# [ADVANCED] This represents the initial value for the state estimate error covariance matrix.

initial_estimate_covariance: [1,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    1,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    1,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    1, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]

As requested you will find below a sample message from each sensor input:

Fix

header: 
  seq: 4502
  stamp: 
    secs: 1691504843
    nsecs: 100990753
  frame_id: "fjcruiser/gnss1_antenna_wgs84"
status: 
  status: 0
  service: 1
latitude: 24.4363592
longitude: 54.6110056
altitude: -19.088
position_covariance: [0.4342809729757313, 0.0, 0.0, 0.0, 0.4342809729757313, 0.0, 0.0, 0.0, 0.7259039967498779]
position_covariance_type: 2

imu

header: 
  seq: 136324
  stamp: 
    secs: 1691505083
    nsecs:  75626252
  frame_id: "fjcruiser/sensor_wgs84"
orientation: 
  x: -0.42456197949594277
  y: 0.9053296474668617
  z: -0.004811025879026105
  w: -0.010112218289144895
orientation_covariance: [1.2389542403262732e-06, 0.0, 0.0, 0.0, 1.2375152844005589e-06, 0.0, 0.0, 0.0, 1.5143168398652242e-05]
angular_velocity: 
  x: -0.00975768268108368
  y: 0.01087676640599966
  z: 0.0530206598341465
angular_velocity_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
linear_acceleration: 
  x: 0.7374916076660156
  y: 0.49138200283050537
  z: -10.263421058654785
linear_acceleration_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]

Wheel_encoder:

header: 
  seq: 62
  stamp: 
    secs: 1691504859
    nsecs: 601004771
  frame_id: "fjcruiser/base_footprint"
child_frame_id: "fjcruiser/base_footprint"
pose: 
  pose: 
    position: 
      x: 0.0
      y: 0.0
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.0
      w: 0.0
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
twist: 
  twist: 
    linear: 
      x: 0.00014493087447644442
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

And as requested this is my TF launch file:

    <launch>
        <arg name="namespace" default="" />
        <arg name="robot_tf_prefix" default="$(arg namespace)/"/>
        <arg name="type" default="$(arg type)/" /> <!-- real, simulation or rosbag-->
    
        <group if="$(eval arg('namespace') == 'sbuggy')"> 
    
            <node pkg="tf2_ros" type="static_transform_publisher" name="$(arg namespace)_link_broadcaster" args="0.375 0 2.29 0 0 0 $(arg namespace)/base_footprint $(arg namespace)/base_link" />  
            <node pkg="tf2_ros" type="static_transform_publisher" name="$(arg namespace)_linkcenter_broadcaster" args="0 0 0 0 0 0 $(arg namespace)/base_footprint $(arg namespace)/center_link" /> 
            <node pkg="tf2_ros" type="static_transform_publisher" name="$(arg namespace)_link1_broadcaster" args="0 0 0 0 0 0 $(arg namespace)/base_link $(arg namespace)/os1/os_sensor" /> 
            <node pkg="tf2_ros" type="static_transform_publisher" name="$(arg namespace)_link2_broadcaster" args="1.10 -0.48 -0.71 0.01151917 0.02600541 0.02600541 $(arg namespace)/base_link $(arg namespace)/os2/os_sensor" /> 
            <node pkg="tf2_ros" type="static_transform_publisher" name="$(arg namespace)_link3_broadcaster" args="1.10 0.56 -0.71 0.02722714 0.02234021 0.04258603 $(arg namespace)/base_link $(arg namespace)/os3/os_sensor" />
            <node pkg="tf2_ros" type="static_transform_publisher" name="$(arg namespace)_link4_broadcaster" args="0 0 0 0 0 0 $(arg namespace)/base_link $(arg namespace)/gnss1_antenna_wgs84" />
            <node pkg="tf2_ros" type="static_transform_publisher" name="$(arg namespace)_link5_broadcaster" args="0 0 0 0 3.14 0 $(arg namespace)/base_link $(arg namespace)/sensor_wgs84" />
      </group>
</launch>

Observations and Testing:

• CPU Load: Always remains below 10% across all cores.
• EKF local Frequency: The limiting frequency of around 80/85 hz remains consistent, even when input frequencies are varied.
• Compilation and Debugging: The package is compiled in release mode, and the debug parameter is set to false.
• Input Queue Sizes: Altering the sizes doesn't have a discernable impact on frequency.
• EKF Dependency: The frequency of ekf_global always matches the maximum frequency of ekf_local. If I set the ekf_local to 10 Hz, I will have also 10Hz on the ekf_global regardless of the asked frequency.
• TF Frequency: The frequency of the /tf topic is the combined frequency of the odom and relative_odom topics.
• TF Interactions: Both EKF nodes have interdependent TF requirements. However, when I replaced the TF produced by ekf_global with a static TF, there was no improvement on frequency.

Request: I'm keen on understanding the reasons behind the frequency limitation of the ekf_local nodes and any potential solutions or workarounds that can be recommended. thank you.

Answer 1

EDIT: I do apologise for the massive delay in looking at this. I don't get a lot of time to do support for r_l these days.

However, I have good news for you.

For background, as with many systems, the time stamps that are coming in from your various data sources are not always consistent. In your case, this is happening:

• In cycle 1, the EKF receives an IMU measurement with timestamp 10
• The EKF does its fusion, and outputs a message with timestamp 10
• In cycle 2, it receives a velocity measurement with timestamp < 10
• This forces the EKF to do a correction in which it rewinds the state, inserts your new measurement, then fuses them all again
• What you might expect now is that the EKF publishes its output again here (note it would publish with timestamp 10 again).

This is where things get tricky. By default, the EKF will not publish these "corrected" states. The reason for this is that if tf2 receives a transform at a time stamp for which it already has a transform, it will spit out a warning. The reason it warns you is that that information is ignored by tf2 internally; it doesn't support updating transforms in the transform tree. There was a long discussion about this here, but the short version is that we added a flag to r_l to prevent it publishing messages with duplicate transforms.

So what's happening is that for most of your cycles, your measurements are coming in in order, but once in a while, one of the EKF update cycles gets an out-of-order measurement, and so it does its rewind and fusion, but does not publish.

It also explains why your second EKF instance is even slower: the additional measurement source might also be generating old messages, so even more of your update cycles are ignored.

Fortunately for you, there are two fixes:

1. Fix the nodes generating your IMU and odometry measurements so that they publish very accurate time stamps, and they publish as soon as possible after obtaining their data.
2. If you really just need the output frequency to be correct, add this to your EKF config:

permit_corrected_publication: true

When I changed this parameter, I got the correct output frequencies.

$ rostopic hz /fjcruiser/odom 
subscribed to [/fjcruiser/odom]
WARNING: may be using simulated time
average rate: 100.295
    min: 0.000s max: 0.050s std dev: 0.01685s window: 100

---

Original response:

I would ask that you please edit your question and add a sample message from each sensor input. In the meantime, I would recommend two things:

1. You should not fuse the output of the odom frame EKF into the map frame instance. Instead, just fuse the same sensor inputs in both instances, but add the GPS data to the second instance. This won't have any bearing on your odom frame EKF instance output rate, but the current setup will create an artificial lag in your map frame state estimate.
2. I am not clear on why the odom frame instance would output at only 85 Hz. I've fused data from multiple IMUs running at 100 Hz without the node breaking a sweat. Off the top of my head, one thing I would recommend strongly is that you make sure the timestamps on all the sensor inputs are absolutely correct. If they aren't, then you can get into this situation:

• Over 25 cycles, we process 25 IMU measurements
• We received one wheel encoder message with a timestamp that is delayed by 0.3 seconds.
• The filter now has to look up the last posterior state that preceded that measurement, insert it, and then re-fuse all the IMU messages from that time until the current time.

Have you tried just fusing the IMU data? Obviously that won't work for a state estimate, and the covariance will explode rapidly, but it would be interesting to see if the output frequency behaves better.

The output rate of the map frame instance being tied to the output rate of the odom frame instance makes sense, as we have to look up the odom->base_link transform in the map frame instance before we can publish map->odom. That lookup has a timeout parameter (you have it set to 0.1), and so the node will block until that transform is available for the time in question.

EDIT after requested information was provided:

• The fact that the frequency increased when you changed the sensor timeout tells me this is likely to do with your input data. But that is definitely the wrong way to do this. You're forcing the EKF to do an update when there should be no need to.
• Let's focus on your "local" EKF instance first. You specify the world_frame to be odom, but you have no parameter for the base_link_frame. That means it will default to base_link. The velocity in your wheel encoder data (which you are trying to fuse in the EKF) is given in the fjcruiser/base_footprint frame. As noted in the r_l documentation, you need to provide a transform from base_link to fjcruiser/base_footprint. Where is that transform being provided? Are you using a static transform publisher? If so, can you provide the launch config you are using? Or are you perhaps using a URDF that defines those transforms?
• Same goes for your IMU data. You gave it a frame_id of fjcruiser/sensor_wgs84, but you are asking it to fuse angular Z velocity from the IMU, and I don't see a transform provided from base_link to fjcruiser/sensor_wgs84.