Random Heading Robot Localization
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 ...
Can you provide a sample message of each input?
Yea, I'm just using the standard ones that are accepted by robot_localization:
IMU: sensor_msgss/Imu
DVL: geometry_msgs/TwistWithCovarianceStamped
Depth: gemoetry_msgs/PoseWithCovarianceStamped
(more)Additionally, here's my launch file:
Please add information by editing your question rather than adding a comment/answer. That way, all of your information will be kept together, and you won't be restricted by the character limit of comments. You'll also get to see a preview of your content, which is nice!
Add
use_sim_time=true
in your launch fileWhat will the addition of use_sim_time do to help here?
I am not sure it will solve your issue but you should definitely do this if you are running packages through simulated data. Otherwise, it could cause your system to mess up current time with simulated time and not give a good output. Let me know if this helps