Revision history [back]

The behavior you're seeing is not drift, and isn't random. The sudden jumps in your pose are from the GPS. As for the path not being straight, have you plotted your raw GPS points? This is what they look like:

The robot starts on the left side and moves to the right. You can see there the GPS points are quite noisy and jump around a lot, especially in the middle (the largest single jump is over 3 meters laterally).

Here is the output I get with your launch file and bag. Red is odom EKF, blue is the map EKF, yellow is the GPS odometry. The first image is in the odom frame:

The second image is in the map frame:

The issue in this case is that your IMU does not agree with your GPS tracks. Your GPS tracks clearly show that the robot had a heading of around +8 degrees with respect to East, and -82 degrees with respect to North. However, the IMU, which I believe you said points 0 at North, is reporting between -14 and -30 degrees during the bag file.

The behavior you're seeing is not drift, and isn't random. The sudden jumps in your pose are from the GPS. As for the path not being straight, have you plotted your raw GPS points? This is what they look like:

The robot starts on the left side and moves to the right. You can see there the GPS points are quite noisy and jump around a lot, especially in the middle (the largest single jump is over 3 meters laterally).

Here is the output I get with your launch file and bag. Red is odom EKF, blue is the map EKF, yellow is the GPS odometry. The first image is in the odom frame:

The second image is in the map frame:

The issue in this case is that your IMU does not agree with your GPS tracks. Your GPS tracks clearly show that the robot had a heading of around +8 degrees with respect to East, and -82 degrees with respect to North. However, the IMU, which I believe you said points 0 at North, is reporting between -14 and -30 degrees during the bag file.

The behavior you're seeing is not drift, and isn't random. The sudden jumps in your pose are from the GPS. As for the path not being straight, have you plotted your raw GPS points? This is what they look like:

The robot starts on the left side and moves to the right. You can see there the GPS points are quite noisy and jump around a lot, especially in the middle (the largest single jump is over 3 meters laterally).

Here is the output I get with your launch file and bag. Red is odom EKF, blue is the map EKF, yellow is the GPS odometry. The first image is in the odom frame:

The second image is in the map frame:

The issue in this case is that your IMU does not agree with your GPS tracks. Your GPS tracks clearly show that the robot had a heading of around +8 degrees with respect to East, and -82 degrees with respect to North. However, the IMU, which I believe you said points 0 at North, is reporting between -14 and -30 degrees during the bag file.

EDIT in response to updates:

I generated the images above based on the last bag and launch file you produced. Note that the odom and map EKF instances are consistent. The only thing that is misbehaving is the location of the GPS odometry points. This is because your IMU itself or the settings you used in navsat_transform_node were wrong/not conformant. In other words, everything about that launch file was absolutely fine. The only thing wrong was the IMU data. You don't need to modify the launch configurations or tweak anything else from the launch file that I used. You just need to get your IMU to conform.

The behavior you're seeing is not drift, and isn't random. The sudden jumps in your pose are from the GPS. As for the path not being straight, have you plotted your raw GPS points? This is what they look like:

The robot starts on the left side and moves to the right. You can see there the GPS points are quite noisy and jump around a lot, especially in the middle (the largest single jump is over 3 meters laterally).

Here is the output I get with your launch file and bag. Red is odom EKF, blue is the map EKF, yellow is the GPS odometry. The first image is in the odom frame:

The second image is in the map frame:

The issue in this case is that your IMU does not agree with your GPS tracks. Your GPS tracks clearly show that the robot had a heading of around +8 degrees with respect to East, and -82 degrees with respect to North. However, the IMU, which I believe you said points 0 at North, is reporting between -14 and -30 degrees during the bag file.

EDIT in response to updates:

I generated the images above based on the last bag and launch file you produced. Note that the odom and map EKF instances are consistent. The only thing that is misbehaving is the location of the GPS odometry points. This is because your IMU itself or the settings you used in navsat_transform_node were wrong/not conformant. In other words, everything about that launch file was absolutely fine. The only thing wrong was the IMU data. You don't need to modify the launch configurations or tweak anything else from the launch file that I used. You just need to get your IMU to conform.

EDIT2 in response to updates:

That all looks correct to me. Explanations:

Why the husky wheel odometry doesn't match This is because you are integrating velocities in the EKF. The EKF starts at (0, 0) and integrates the Husky's velocity data. The Husky's wheel odometry will not have started at (0, 0), and isn't using an IMU, so the heading won't agree either. As long as they have the same general "shape," it's fine. In other words, it's completely normal for the wheel odometry to not agree with the EKF output when you are only fusing velocities.

Why the GPS odometry doesn't line up perfectly First, note that the direction and path shape do match for the GPS odometry data, so I think you've got the heading sorted out nicely. I think it looks pretty good; that static offset could just be error from the GPS itself. I would plot the raw lat/longs using Google Earth and the GPS Visualizer website.

Note that the GPS poses have no heading, so they are all facing a heading of 0.

header: 
  seq: 38723
  stamp: 
    secs: 1491907055
    nsecs: 615744113
  frame_id: base_imu
orientation: 
  x: -0.00451018242165
  y: 0.00442147208378
  z: -0.858403027058
  w: 0.512937366962
orientation_covariance: [0.017453292519943295, 0.0, 0.0, 0.0, 0.017453292519943295, 0.0, 0.0, 0.0, 0.15707963267948966]
angular_velocity: 
  x: -0.00197708723135
  y: -0.0037388484925
  z: -0.000131436216179
angular_velocity_covariance: [0.0004363323129985824, 0.0, 0.0, 0.0, 0.0004363323129985824, 0.0, 0.0, 0.0, 0.0004363323129985824]
linear_acceleration: 
  x: 0.0753428861499
  y: -0.147220656276
  z: 9.80377960205
linear_acceleration_covariance: [0.0004, 0.0, 0.0, 0.0, 0.0004, 0.0, 0.0, 0.0, 0.0004]

The behavior you're seeing is not drift, and isn't random. The sudden jumps in your pose are from the GPS. As for the path not being straight, have you plotted your raw GPS points? This is what they look like:

The robot starts on the left side and moves to the right. You can see there the GPS points are quite noisy and jump around a lot, especially in the middle (the largest single jump is over 3 meters laterally).

Here is the output I get with your launch file and bag. Red is odom EKF, blue is the map EKF, yellow is the GPS odometry. The first image is in the odom frame:

The second image is in the map frame:

The issue in this case is that your IMU does not agree with your GPS tracks. Your GPS tracks clearly show that the robot had a heading of around +8 degrees with respect to East, and -82 degrees with respect to North. However, the IMU, which I believe you said points 0 at North, is reporting between -14 and -30 degrees during the bag file.

EDIT in response to updates:

I generated the images above based on the last bag and launch file you produced. Note that the odom and map EKF instances are consistent. The only thing that is misbehaving is the location of the GPS odometry points. This is because your IMU itself or the settings you used in navsat_transform_node were wrong/not conformant. In other words, everything about that launch file was absolutely fine. The only thing wrong was the IMU data. You don't need to modify the launch configurations or tweak anything else from the launch file that I used. You just need to get your IMU to conform.

EDIT2 in response to updates:

That all looks correct to me. Explanations:

Why the husky wheel odometry doesn't match This is because you are integrating velocities in the EKF. The EKF starts at (0, 0) and integrates the Husky's velocity data. The Husky's wheel odometry will not have started at (0, 0), and isn't using an IMU, so the heading won't agree either. As long as they have the same general "shape," it's fine. In other words, it's completely normal for the wheel odometry to not agree with the EKF output when you are only fusing velocities.

Why the GPS odometry doesn't line up perfectly First, note that the direction and path shape do match for the GPS odometry data, so I think you've got the heading sorted out nicely. I think it looks pretty good; that static offset could just be error from the GPS itself. I would plot the raw lat/longs using Google Earth and the GPS Visualizer website.

Note that the GPS poses have no heading, so they are all facing a heading of 0.

EDIT3

Your IMU data is still not correct. Here is a plot of your GPS tracks:

The yellow pin is your start location. You clearly moved in a north-east north-west trajectory, yet here is a sample of your IMU data:

header: 
  seq: 38723
  stamp: 
    secs: 1491907055
    nsecs: 615744113
  frame_id: base_imu
orientation: 
  x: -0.00451018242165
  y: 0.00442147208378
  z: -0.858403027058
  w: 0.512937366962
orientation_covariance: [0.017453292519943295, 0.0, 0.0, 0.0, 0.017453292519943295, 0.0, 0.0, 0.0, 0.15707963267948966]
angular_velocity: 
  x: -0.00197708723135
  y: -0.0037388484925
  z: -0.000131436216179
angular_velocity_covariance: [0.0004363323129985824, 0.0, 0.0, 0.0, 0.0004363323129985824, 0.0, 0.0, 0.0, 0.0004363323129985824]
linear_acceleration: 
  x: 0.0753428861499
  y: -0.147220656276
  z: 9.80377960205
linear_acceleration_covariance: [0.0004, 0.0, 0.0, 0.0, 0.0004, 0.0, 0.0, 0.0, 0.0004]

That orientation corresponds to a yaw of -114.061 degrees. That is clearly not correct. If your IMU reported 0 facing east, then I would expect its heading to be closer to +114 degrees in that image. If it reported 0 facing north, it would report something like +25 degrees. Your configuration has a yaw offset of pi/2, so that means your IMU should report 0 facing north. Are you certain that the IMU is reporting data in ENU, and not NED?

Your IMU heading should increase as you rotate counter-clockwise. I would do the following:

1. Make your IMU face north (i.e., the X-axis on the IMU drawing should face north). Take a reading using the ROS driver output (the /imu/data message). Convert it to Euler angles here. Write down the yaw.
2. Make your IMU face west. Again, convert the quaternion to Euler angles and write down the yaw.
3. Make your IMU face south. Convert and write down.
4. Make your IMU face east. Convert and write down.