UKF NaNs - Githubissues

Describe the bug

After #751 was merged, the Unscented Kalman Filter starts producing NaN values within about 5 seconds when the simulation is paused, and no new sensor readings are being published.

Once this happens, this message will appear:

https://github.com/cra-ros-pkg/robot_localization/blob/ef0a27352962c56a970f2bbeb8687313b9e54a9a/src/ros_filter.cpp#L2157-L2163

...and then the terminal will be flooded with TF NaN errors.

This issue does not occur when using the Extended Kalman Filter.

To Reproduce

Steps to reproduce the behavior:

Run a simulated robot and launch robot_localization.
The filter will properly work and estimate the position as usual.
Pause the simulation so that no new sensor data is received, also attempt driving around or resuming a few times.
At some point a load of TF NaN errors will appear in the terminal.

Expected behavior

The UKF should never produce NaN values.

Desktop (please complete the following information):

OS: Ubuntu 24.04.1 LTS
ROS Distribution: Jazzy
robot_localization Package Version: jazzy-devel @ 0cd2e468493325754bd1e7add7384df75d9f1658 Added subscription to stamped topic

Additional context

We are currently using the EKF as a workaround. Below is the UKF configuration we used on Humble without issues:

/**:
  ros__parameters:
    frequency: 30.0
    sensor_timeout: 0.1
    two_d_mode: false
    transform_time_offset: 0.0
    transform_timeout: 0.0
    print_diagnostics: false
    debug: false
    permit_corrected_publication: false
    publish_acceleration: false
    publish_tf: true

    map_frame: map
    odom_frame: odom
    base_link_frame: base_link
    world_frame: odom

{% for camera_id in rgbd_ids %}
    odom{{ loop.index0 }}: {{ camera_id }}/odom
    odom{{ loop.index0 }}_config: [false, false, false,
                                   false, false, false,
                                   true,  true,  true,
                                   true,  true,  true,
                                   false, false, false]
    odom{{ loop.index0 }}_queue_size: 10
    odom{{ loop.index0 }}_differential: false
    odom{{ loop.index0 }}_relative: false
{% endfor %}

    imu0: imu/data
    imu0_config: [false, false, false,
                  true,  true,  true,
                  false, false, false,
                  false, false, false,
                  false, false, false]
    imu0_differential: false
    imu0_relative: false
    imu0_queue_size: 10
    imu0_remove_gravitational_acceleration: true

    use_control: false

    process_noise_covariance: [
      0.05, 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.05, 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.06, 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,  10.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,  10.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,  10.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.025, 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.025, 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.04, 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.01, 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.01, 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.02, 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.01, 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.01, 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.015
    ]
    # NOTE: Setting 10.0 back to 0.03 did not help with the NaNs.

Regarding Deprecation

In the issue template I also noticed the comment mentioning that the robot_localization package will soon be deprecated. This package is widely used and popular in the ROS community, so I have trouble understanding why it is being deprecated? Is there a planned replacement?

cra-ros-pkg / robot_localization