ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Random Heading Robot Localization

asked 2021-04-08 15:22:42 -0500

tdoe321 gravatar image

updated 2021-04-20 12:52:47 -0500

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 ...
(more)
edit retag flag offensive close merge delete

Comments

Can you provide a sample message of each input?

xaru8145 gravatar image xaru8145  ( 2021-04-09 08:51:55 -0500 )edit

Yea, I'm just using the standard ones that are accepted by robot_localization:

IMU: sensor_msgss/Imu

header: 
  seq: 0
  stamp: 
    secs: 564
    nsecs: 929049968
  frame_id: "imu_frame"
orientation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 0.0
orientation_covariance: [0.2, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.2]
angular_velocity: 
  x: 0.0207530661947
  y: 0.0583150693847
  z: 0.00502123484901
angular_velocity_covariance: [0.2, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.2]
linear_acceleration: 
  x: -0.792346715927
  y: 0.0506370328367
  z: -0.612429082394
linear_acceleration_covariance: [0.2, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.2]
tdoe321 gravatar image tdoe321  ( 2021-04-09 09:14:24 -0500 )edit

DVL: geometry_msgs/TwistWithCovarianceStamped

header: 
  seq: 0
  stamp: 
    secs: 5382
    nsecs: 56898999
  frame_id: "dvl_frame"
twist: 
  twist: 
    linear: 
      x: -0.097
      y: 0.019
      z: 0.037
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2]

Depth: gemoetry_msgs/PoseWithCovarianceStamped

header: 
  seq: 0
  stamp: 
    secs: 646
    nsecs: 39082026
  frame_id: "odom"
pose: 
  pose: 
    position: 
      x: 0.0
      y: 0.0
      z: -15.1876435961
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.0
      w: 0.0
  covariance: [0.15, 0 ...
(more)
tdoe321 gravatar image tdoe321  ( 2021-04-09 09:16:38 -0500 )edit

Additionally, here's my 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 base_link imu_frame" />

    <node pkg="rosbag" type="play" name="player" output="screen" args="$(find nav_filter)/bags/$(arg playback_name) -r 100"/>
</launch>
tdoe321 gravatar image tdoe321  ( 2021-04-09 09:20:42 -0500 )edit

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!

tryan gravatar image tryan  ( 2021-04-09 10:10:03 -0500 )edit
1

Add use_sim_time=true in your launch file

xaru8145 gravatar image xaru8145  ( 2021-04-09 10:25:29 -0500 )edit

What will the addition of use_sim_time do to help here?

tdoe321 gravatar image tdoe321  ( 2021-04-09 10:32:48 -0500 )edit

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

xaru8145 gravatar image xaru8145  ( 2021-04-10 07:02:57 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2021-04-27 04:25:34 -0500

Tom Moore gravatar image

Are all of the inputs available and publishing immediately as the node starts? If one of those data sources starts some number of seconds after the filter is initialized, then you might see the filter's state "exploding" briefly before everything converges. Can you echo the output filtered topic and try playing back your bag very slowly (or stepping through by playing back with --pause and then pressing s to force the bag playback to move to just the next message), and see what the first few outputs look like?

edit flag offensive delete link more

Question Tools

3 followers

Stats

Asked: 2021-04-08 15:22:42 -0500

Seen: 303 times

Last updated: Apr 27 '21