1
$\begingroup$

I'm in the process of tuning a robot-localization package parameters in my sensor-fusion ROS2 package. I have a dataset from various sensors mounted on a vehicle (IMU, GNSS, Lidar, Rgb Cameras, Radar, ...) without a direct odometry input from the vehicle.

The RTK GNSS I'm using has a 20Hz output, Imu publishes with a frequency of > 300Hz. That said, I'm using a combo of ekf_node and navsat_transform_node according to tutorial where the GNSS is converted into odometry/gps relatively. I've got no map frame.

I'd like to maximize the output frequency of the package possibly up to the frequency of the IMU if this can be handled computationally (or if it even makes sense?).

I've been increasing the frequency param of the ekf_filter_node, but at around 60Hz the actual output drops down abruptly to only around 5Hz. Fiddling around with parameters that I've felt could decrease the computational time yields basically no additional FPS.

This is my config file and a relevant part of the launch file:

ekf_filter_node:
  ros__parameters:
    frequency: 100.0
    sensor_timeout: 0.001
    two_d_mode: false
    reset_on_time_jump: true

    print_diagnostics: false
    debug: false

    publish_tf: true
    publish_acceleration: false

    odom_frame: odom
    base_link_frame: imu
    world_frame: odom

    odom0: odometry/gps
    odom0_config: [ true,  true,  true,
                    false, false, false,
                    false, false, false,
                    false, false, false,
                    false, false, false ]
    odom0_queue_size: 1
    odom0_nodelay: false
    odom0_differential: false
    odom0_relative: true

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

    use_control: false

    process_noise_covariance: [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.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.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.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.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.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.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.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.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.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.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.1]

    initial_estimate_covariance: [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,    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,    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,    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,    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,    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,    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,    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,    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,    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,     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,     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,     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,    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,    1.0]

navsat_transform_node:
  ros__parameters:
    frequency: 20.0
    delay: 0.0
    magnetic_declination_radians: 0.08726646  # For lat/long 49.2, 16.6
    yaw_offset: 1.570796327  # IMU reads 0 facing magnetic north, not east
    zero_altitude: false
    broadcast_cartesian_transform: false
    publish_filtered_gps: false
    use_odometry_yaw: false
    wait_for_datum: false

.

        Node(
            package='robot_localization',
            executable='ekf_node',
            name='ekf_filter_node',
            output='screen',
            parameters=[os.path.join(pathlib.Path().resolve(), 'params', 'ekf.yaml')],
            remappings=[('imu/data', config_yaml["raw_topics"]["imu"]["imu"])]
        ),
        Node(
            package='robot_localization',
            executable='navsat_transform_node',
            name='navsat_transform_node',
            output='screen',
            parameters=[os.path.join(pathlib.Path().resolve(), 'params', 'ekf.yaml')],
            remappings=[('imu', config_yaml["raw_topics"]["imu"]["imu"]),
                        ('gps/fix', config_yaml["raw_topics"]["gnss"]["position"])]

        )

I'm also adding the actual framerate and ROS2 graph as reported by the rqt

enter image description here

enter image description here

Happy to get any suggestions as to what could the bottleneck be and why the FPS drops significantly when I ask for ~60Hz and more.

$\endgroup$
1
  • $\begingroup$ Can you provide some sample sensor messages for each sensor input, and also a sample EKF message after the filter slows down to 5 Hz? $\endgroup$
    – automatom
    Feb 15 at 16:02

1 Answer 1

0
$\begingroup$

Without more information (see my comment on your question), my two guesses are:

  1. The time stamps on your IMU data and GPS data don't agree. Try setting permit_corrected_publication: true, and see this question.
  2. You have some sort of covariance issue somewhere that is causing numerical issues.
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.