0
$\begingroup$

I want to use Nav2 for robot navigation. The robot that I am using is a skid-steer-tracked robot that does not provide reliable odometry data. I am trying to use the robot_localization package to get the odometry from the imu and then use it as an input to nav2 using a laser scanner. I am not getting any messages published on the odometry/filtered topic of the robot_localization package but I get the following message on the /diagnostics topic. Could someone please guide me on this issue?

header:
  stamp:
    sec: 1723455685
    nanosec: 294059951
  frame_id: ''
status:
- level: "\x02"
  name: 'ekf_filter_node: Filter diagnostic updater'
  message: Erroneous data or settings detected for a robot_localization state estimation this->
  hardware_id: none
  values:
  - key: X_configuration
    value: Neither X nor its velocity is being measured. This will result in unboundederror growth and erratic filter behavior.
  - key: Y_configuration
    value: Neither Y nor its velocity is being measured. This will result in unboundederror growth and erratic filter behavior.
  - key: Z_configuration
    value: Neither Z nor its velocity is being measured. This will result in unboundederror growth and erratic filter behavior.
- level: "\x02"
  name: 'ekf_filter_node: odometry/filtered topic status'
  message: No events recorded.
  hardware_id: none
  values:
  - key: Events in window
    value: '0'
  - key: Events since startup
    value: '0'
  - key: Duration of window (s)
    value: '5.066757'
  - key: Actual frequency (Hz)
    value: '0.000000'
  - key: Minimum acceptable frequency (Hz)
    value: '25.200000'
  - key: Maximum acceptable frequency (Hz)
    value: '35.200000'

Here is my ekf params file for reference:

### ekf config file ###
ekf_filter_node:
    ros__parameters:
        frequency: 30.0
        sensor_timeout: 0.1
        two_d_mode: true
        transform_time_offset: 0.0
        transform_timeout: 0.0
        print_diagnostics: true
        debug: false
        debug_out_file: /path/to/debug/file.txt
        permit_corrected_publication: false
        publish_acceleration: false
        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

        imu0: imu/data
        imu0_config: [false, false, false,
                      true,  true,  true,
                      false, false, false,
                      true,  true,  true,
                      true,  true,  true]
        imu0_differential: false
        imu0_relative: true
        imu0_queue_size: 5
        imu0_pose_rejection_threshold: 0.8                 #
        imu0_twist_rejection_threshold: 0.8                #
        imu0_linear_acceleration_rejection_threshold: 0.8  #
        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,    0.03, 0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.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,    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,    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]

        initial_estimate_covariance: [1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9]
$\endgroup$
1
  • $\begingroup$ Steve answered this below, but the filter is telling you why it's unhappy. You don't have any sensor measuring linear velocity or linear position. The double-integration of linear acceleration will result in unbounded error growth that will quickly cause your state estimate to diverge. Even if the wheel odometry is bad on your platform, it's better than none at all. $\endgroup$
    – automatom
    Commented Oct 28 at 11:57

1 Answer 1

1
$\begingroup$

You want to also integrate in your tracked wheel odometry to get translational motion. Right now with just the IMU, you only have orientation information without translational data. The IMU's acceleration should probably be turned off unless you have a really, really expensive IMU. Accelerations alone aren't adiqute for translational state estimation. Double integration is really not awesome.

value: Neither X nor its velocity is being measured. This will result in unboundederror growth and erratic filter behavior.

If you're not going to use any other sources, you could just use your IMU's fused orientation vector, you don't really need robot_localization for just an IMU that has pre-filtered absolute orientation data (which you have all set to True so I imagine its available).

imu/data --> is this the right topic that your IMU data is being published on? This error makes me think the topic is wrong

  name: 'ekf_filter_node: odometry/filtered topic status'
  message: No events recorded.
$\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.