Context & Hardware Setup

ROS ROS General
, , ,
1

I am developing an Autonomous Mobile Robot (AMR) designed to operate inside a dairy cattle barn. The locomotion system suffers from high wheel slippage due to the environment’s muddy, uneven, and unpaved terrain.

My current sensor suite consists of:

  • Wheel Encoders (Differential drive odometry)

  • IMU (Providing angular velocity and absolute orientation)

  • Ultra-Wideband (UWB) Positioning System (1 Tag on the robot, multiple Anchors around the barn)

  • (Upcoming) Range sensors for obstacle avoidance (cattle detection).

For state estimation, I am using the robot_localization package running a Dual EKF setup to broadcast the map -> odom -> base_footprint transform tree.

The Problem

I am experiencing significant localization inaccuracies, specifically with the robot’s heading (yaw) and absolute positioning.

  1. Heading / Yaw Errors: The estimated orientation is frequently wrong or drifts rapidly. The barn structure contains dense steel pillars (headlocks and support frames) which I suspect are causing massive magnetic disturbances to the IMU’s magnetometer.

  2. UWB Inaccuracy & Jumps: Initially, the UWB update rate was 1 Hz, which caused huge position jumps. I managed to optimize the anchor-tag communication to achieve around 10 Hz. While the frequency issue is resolved, I still observe positional errors between 10 cm to 40 cm, which drastically degrade especially when the robot approaches the steel pillars or when cattle walk between the tag and the anchors.

Current Configuration & Attempts

  • The first EKF instance fuses wheel odometry and IMU (angular velocity) to output continuous odom -> base_footprint.

  • The second EKF instance fuses the odom data and absolute UWB positions to output map -> odom.

  • I have noticed that high multipath reflections and Non-Line-of-Sight (NLOS) conditions caused by steel structures and livestock are heavily corrupting the UWB ranges.

Questions for the Community

Given the harsh environment (muddy terrain, heavy metal interference, dynamic obstacles like cows), I would highly appreciate any advice or architectural suggestions:

  1. How should I handle the IMU in a high-metal environment? Should I completely disable the magnetometer fusion in robot_localization and rely solely on differential_drive odometry + IMU angular velocity (z-axis gyro) for heading, even if it drifts over time?

  2. How can I mitigate UWB multipath/NLOS errors in EKF? Are there proven outlier rejection methods or thresholding techniques within robot_localization (like using the covariance matrix dynamically or mahalanobis_distance filtering) to ignore bad UWB fixes near the steel pillars?

  3. Alternative Fusion Strategies: Has anyone successfully implemented robust UWB-based localization in similar agricultural setups? Would moving towards an Unscented Kalman Filter (UKF) or a factor-graph-based optimization approach (like Ceres or GTSAM) yield better results than standard Dual EKF?

Thank you in advance for your insights!

My ekf configuration :

odometry_ekf_filter_node:
    ros__parameters:
        sensor_timeout: 0.1
# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
# computation until it receives at least one message from one of the inputs. It will then run continuously at the
# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
        frequency: 30.0
        permit_corrected_publication: false
# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
# by, for example, an IMU. Defaults to false if unspecified.
        two_d_mode: true

# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for
# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if
# unspecified.
        transform_time_offset: 0.0

# Use this parameter to provide specify how long the tf listener should wait for a transform to become available.
# Defaults to 0.0 if unspecified.
        transform_timeout: 0.05

# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is
# unhappy with any settings or data.
        print_diagnostics: false

# Whether to publish the acceleration state. Defaults to false if unspecified.
        publish_acceleration: false

# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
        publish_tf: true
        # initial_state: [2.0,  -8.0,  0.0,
        #             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. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
#     1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of odom_frame.
# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set "world_frame"
#    to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates from landmark
#    observations) then:
#     3a. Set your "world_frame" to your map_frame value
#     3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state estimation node
#         from robot_localization! However, that instance should *not* fuse the global data.
        #map_frame: map              # Defaults to "map" if unspecified
        odom_frame: odom            # Defaults to "odom" if unspecified
        base_link_frame: base_footprint  # Defaults to "base_link" if unspecified
        world_frame: odom           # Defaults to the value of odom_frame if unspecified

        odom0: /wheel_odom
        # x, y pozisyon differential modda fuse ediliyor.
        # Twist (vx) kapalı - twist verisine güvenilmiyor.
        # differential: true -> mutlak pose değil, adım adım delta alınıyor,
        # böylece birikimli odometri hatası direkt EKF'e yansımaz.
        odom0_config: [true, true, false,
                      false, false, false,
                      false, false, false,
                      false, false, false,
                      false, false, false]
        odom0_differential: true
        odom0_relative: false
        odom0_queue_size: 10
        odom0_pose_rejection_threshold: 5.0
        odom0_twist_rejection_threshold: 5.0

        imu0: /witmotion/imu
        imu0_config: [false, false, false,
                      false, false, false,  # absolute yaw KAPALI — motor EMI manyetometreyi bozuyor, gradual drift yaratıyor
                      false, false, false,
                      false, false, true,   # yaw_rate (jiroskop) — SLAM map→odom global yaw'ı düzeltiyor
                      false, false, false]
        imu0_differential: false
        imu0_relative: false
        imu0_queue_size: 10
        # EMI koruması: 10.0'dan 3.0'a düşürüldü.
        # Motorlardan kaynaklanan magnetik spike'lar bu eşiği aşarsa elenir.
        # Eğer yaw sürekli reddediliyorsa (diagnostics'te görünür) değeri
        # biraz artır (4.0-5.0) ya da imu0_config[5]'i false yap.
        imu0_pose_rejection_threshold: 3.0
        imu0_twist_rejection_threshold: 5.0
        imu0_linear_acceleration_rejection_threshold: 0.8
        imu0_remove_gravitational_acceleration: true

ekf_filter_node:
  ros__parameters:
    frequency: 10.0
    sensor_timeout: 0.5
    two_d_mode: true          # Set false for 3D navigation (drones, etc.)
    transform_time_offset: 0.0
    transform_timeout: 0.0
    print_diagnostics: true
    debug: false
    publish_tf: false

    map_frame: map
    odom_frame: odom
    base_link_frame: base_footprint
    world_frame: map
    # [x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az]
    # initial_state: [2.55,  -7.4,  0.0,
    #                 0.0,  0.0,  0.0,
    #                 0.0,  0.0,  0.0,
    #                 0.0,  0.0,  0.0,
    #                 0.0,  0.0,  0.0]
    # -----------------------------------------------------------------------
    # Sensor 0: Wheel Odometry
    # -----------------------------------------------------------------------
    # Fuse: vx, vy, vyaw (velocities from wheel encoders)
    # [x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az]
    odom0: /wheel_odom
    # x, y pozisyon differential modda fuse ediliyor.
    # Twist (vx) kapalı - twist verisine güvenilmiyor.
    # differential: true -> mutlak pose değil, adım adım delta alınıyor,
    # böylece birikimli odometri hatası direkt EKF'e yansımaz.
    odom0_config: [true, true, false,
                  false, false, false,
                  false, false, false,
                  false, false, false,
                  false, false, false]
    odom0_differential: true
    odom0_relative: false
    odom0_queue_size: 10
    odom0_pose_rejection_threshold: 5.0
    odom0_twist_rejection_threshold: 5.0

    imu0: /witmotion/imu
    imu0_config: [false, false, false,
                  false, false, false,  # absolute yaw KAPALI — motor EMI manyetometreyi bozuyor, gradual drift yaratıyor
                  false, false, false,
                  false, false, true,   # yaw_rate (jiroskop) — SLAM map→odom global yaw'ı düzeltiyor
                  false, false, false]
    imu0_differential: false
    imu0_relative: false
    imu0_queue_size: 10
    # EMI koruması: 10.0'dan 3.0'a düşürüldü.
    # Motorlardan kaynaklanan magnetik spike'lar bu eşiği aşarsa elenir.
    # Eğer yaw sürekli reddediliyorsa (diagnostics'te görünür) değeri
    # biraz artır (4.0-5.0) ya da imu0_config[5]'i false yap.
    imu0_pose_rejection_threshold: 3.0
    imu0_twist_rejection_threshold: 0.8
    imu0_linear_acceleration_rejection_threshold: 0.8
    imu0_remove_gravitational_acceleration: true

    # -----------------------------------------------------------------------
    # Sensor 1: UWB Trilateration Position
    # -----------------------------------------------------------------------
    # Fuse: x, y, z (absolute position from trilateration)
    # [x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az]
    pose0: /uwb/pose
    pose0_config: [true,  true,  false,
                   false, false, false,
                   false, false, false,
                   false, false, false,
                   false, false, false]
    pose0_differential: false
    pose0_queue_size: 10
    pose0_rejection_threshold: 1.0  # Reject UWB jumps > 2m (NLOS protection)