Random Heading Robot Localization

edit

Hello, I've been using the robot localization package to filter an underwater robot's position over time and I'm getting some good plots of pose out compared to a ground truth validation set. All of my work right now is happening from replaying recorded sensor data bag files. However, it seems that the heading does not always converge to the same value when re-running recorded sensor data through the filter I have set up.

I'm basically re-running the same launch file I have set up and getting a different heading estimation out, but the same x, y movements over time. I do see different actual values for the x and y movement, but that is because the robot's heading is different, and therefore the x, y values will be changed, but the relative x, y is always the same.

On a few different runs (literally just re-launching the launch file) I've gotten the following headings relative to the odom frame: +45 deg, +135 deg, -45 deg. And it appears to be random which one it ends up at.

I have some suspicions of what could be the culprit, but I would like to know if anyone has any thoughts as to why this could be happening. The details of my setup are below:

Sensors

• DVL - providing linear velocities
• IMU - providing angular velocities only (Initial heading and orientation is measured using external compas - I'm setting the initial state using the initial_state param)
• Pressure Sensor - providing depth

YAML Setup

frequency: 30

two_d_mode: false

publish_tf: true

base_link_frame: base_link
odom_frame: odom
world_frame: odom

# From external source
initial_state: [0.0,  0.0,  0.0,
                -0.19,  -1.41,  3.1,
                0.0,  0.0,  0.0,
                0.0,  0.0,  0.0,
                0.0,  0.0,  0.0]

# IMU
imu0: /imu/data
imu0_remove_gravitational_acceleration: false
imu0_config: [false, false, false,
              false, false, false,
              false, false, false,
              true, true, true,
              false, false, false]
imu0_differential: false

# DVL
twist0: /twist/data
twist0_config: [false, false, false,
               false, false, false,
               true, true, true,
               false, false, false,
               false, false, false]
twist0_differential: false

# Depth
pose0: /pose/data
pose0_config: [false, false, true,
               false, false, false,
               false, false, false,
               false, false, false,
               false, false, false]
pose0_differential: false

process_noise_covariance: # Removed for brevity (diagonals set to small values)
initial_estimate_covariance: # Removed for brevity (diagonals set to small values)

Additional Info I have tried to add in the navsat_transform_node also, but have had no luck with that. I will still get headings that are sometimes close to correct and others not.

Launch File

<launch>
    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_from_playback">
        <rosparam command="load" file="$(find nav_filter)/config/DVL_IMU_localization_playback_config.yaml" />
    </node>

    <!-- Upside down DVL & IMU to account for +Z being down and +Y being Starboard (underwater) vs +Z being up and +Y being Port (ROS) -->
    <node pkg="tf2_ros" type="static_transform_publisher" name="dvl_base_link" args="0 0 0 1 0 0 0 base_link dvl_frame" />
    <node pkg="tf2_ros" type="static_transform_publisher" name="imu_base_link" args="0 0 0 1 0 0 0 ...