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]
```
**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.