Yaw problem for IMU fusinon in robot_localization

Question

EDIT1: The Yaw feedback from Mti-300 is a didital compass that in ENU frame. So it is not the same with /odom source. If I fusion data with IMU's Yaw, the result is very bad. If I only fusion with the yaw angular velocity, the result is acceptable. Is it correct? and Could anyone please tell me how to deal with the absolute yaw value? @M@t @Moderator

EDIT2 I've got a GPS　and a Mti-300 IMU producing IMU message at 400HZ. I am trying to use ekf_fusion to estimate the robot's position. Firstly, when I tried to use only one IMU & odom without the GPS, the result is very very bad. But I tried the testbag1, it works fine. Here is my simple launch file. The other launch file for the IMU node is omitted.

<node pkg="rosbag" type="play" name="rosbagplay" args="$(find robot_localization)/test_turtlebot/1202.bag --clock -d 5" required="true"/>

<node name="test_ekf_localization_node_bag2_ekf" pkg="robot_localization" type="ekf_localization_node" clear_params="true" >

  <param name="frequency" value="30"/>

  <param name="sensor_timeout" value="0.1"/>
  

  <param name="odom0" value="/odom"/>
  <param name="imu0" value="/imu/data"/>

  <param name="map_frame" value="map"/>
  <param name="odom_frame" value="odom"/>
  <param name="base_link_frame" value="base_footprint"/>
  <param name="world_frame" value="odom"/>

  <rosparam param="odom0_config">[false, false, false,
                                  false, false, false,
                                  true,  true, true,
                                  false, false, false,
                                  false, false, false]</rosparam>

  <rosparam param="imu0_config">[false, false, false,
                                 true, true, true,
                                 false, false, false,
                                 true, true, true,
                                 true, true, true]</rosparam>

  <param name="odom0_queue_size" value="10"/>
  <param name="imu0_queue_size" value="10"/>

  <param name="imu0_remove_gravitational_acceleration" value="true"/>

I found the problem maybe caused by the yaw output from my IMU. Change the configuration matrix to

  <rosparam param="odom0_config">[false, false, false,
                                  false, false, true,
                                  true,  true, true,
                                  false, false, false,
                                  false, false, false]</rosparam>

  <rosparam param="imu0_config">[false, false, false,
                                 true, true, false,
                                 false, false, false,
                                 true, true, true,
                                 true, true, true]</rosparam> 

The result is better,about 1 m error after 200m testing. However I know it is not corrct for using yaw from /odom. But it sems like I cannot use the yaw(from compass) from IMU.

@Tom Moore Thnks very much, here are my sample messages.

/odom: ---
header: 
  seq: 5419
  stamp: 
    secs: 1480672217
    nsecs: 320610941
  frame_id: odom
child_frame_id: base_footprint
pose: 
  pose: 
    position: 
      x: 4.18018921507
      y: -0.135587119981
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: -0.0435322037951
      w: 0.999052024287
  covariance: [0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7976931348623157e+308, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05]
twist: 
  twist: 
    linear: 
      x: 0.347565259959
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [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.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, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]


/imu
header: 
  seq: 26753
  stamp: 
    secs: 1480672257
    nsecs: 346966028
  frame_id: imu_link
orientation: 
  x: 0.0027948294582
  y: -0.0135141274958
  z: -0.988287867583
  w: 0.151975814111
orientation_covariance: [0.017453292519943295, 0.0, 0.0, 0.0, 0.017453292519943295, 0.0, 0.0, 0.0, 0.15707963267948966]
angular_velocity: 
  x: -0.0103946747258
  y: -0.00381026370451
  z: 0.306645005941
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.331079125404
  y: 0.683972775936
  z: 10.7696285248
linear_acceleration_covariance: [0.0004, 0.0, 0.0, 0.0, 0.0004, 0.0, 0.0, 0.0, 0.0004]

Clearly, if yaw angle comes from IMU, the initial value is not zero, but the yaw angle from /odom is 0.

EDIT3: @M@t @Tom Moore I ran turtlebot at a 4X4 square, here is the bag file link. [https://drive.google.com/file/d/0ByqC7Wzoqm1GSjdKWEZRRW4xdGc/view?usp=sharing] The result is briefly shown here:

[https://drive.google.com/file/d/0ByqC7Wzoqm1GNG9FZm82QlJvVFU/view?usp=sharing]

The blue one is the original /odom from the turtlebot. The red one is from EKF fusion with IMU. The red one cannot come back to the origin. Is it because my IMU isn't good enough? The yaw angle cann't keep all the time?

---

Originally posted by jerryzhchao on ROS Answers with karma: 13 on 2016-11-29

Post score: 1

---

Original comments

Comment by gvdhoorn on 2016-11-29:
Could you please format your code copy/pastes using the Preformatted Text button next time (it's the one with 101010 on it)? Select the code/launch file/console text and press the button (or use ctrl+k).

Thanks.

Comment by jerryzhchao on 2016-11-29:
@gvdhoorn Thanks for reminder.

Comment by Tom Moore on 2016-12-19:
Can you please also post sample input messages for each of your inputs?

Comment by M@t on 2016-12-19:
Can you also confirm that your IMU conforms to REP 103? I.e does the yaw increases CCW and read 0.0 when facing East? If you could post some links to some short bagfiles that would help as well.

Comment by jerryzhchao on 2016-12-19:
@M@t Yes the IMU conforms to REP103 with ENU frame. I will prepare a small bag asap.

Comment by M@t on 2016-12-19:
Awesome. I suggest you drive it in a small square, say 5x5m. Start and finish at the same point. Then we can see how the EKF responds going in straight lines with different headings, and how it responds when turning on the spot. Also make sure your robot is away from any large metal objects.

Answer 1

The Solution (TL;DR)

Your robot is perfectly fine, this is just how ROS deals with odometry data from wheel encoders.

---

The Solution (full explanation)

I think I can explain the behavior you're seeing, and I think that there isn't actually a problem with your setup or your IMU. What you're seeing is just a quirk of how ROS handles odometry data. I'll make the explanation as thorough as I can, because this issue confused me as well when I started working with ROS, so hopefully this will help you and anyone else who reads this.

Let's examine this image, all the data is relative to the odom reference frame. The red axis is 'X' and the green axis is 'Y', blue is 'Z'. It is important to understand that in ROS, world-fixed reference frames like odom and map are always orientated so that the 'Y' axis faces magnetic north:

There are two sets of data here:

1. Blue /odom data This is the original odometry data captured from the turtlebot's wheel encoders. Wheel encoders don't know what direction is north, so this data has no way of orientating itself. From my personal experience working with ROS, all data like this that comes from wheel encoders always starts off facing the 'X' axis of odom and goes from there. So this data will always start facing 'X' regardless of the actual magnetic heading of your turtlebot. My robot does the exact same thing.
2. Red EKF data This is odometry as well, but it's come from an EKF that has fused IMU and wheel encoder data. So thanks to the IMU, this data is orientated relative to the earth's magnetic field. So this data correctly reflects the actual path of your robot in the world. This data doesn't start and end in the same place, but that's perfectly normal because both the IMU and encoder data is relative to the robot's original position, it's not absolute global-referenced data like from a GPS, so the position error accumulates over time.

In short, your robot is perfectly fine. The red data is the EKF's best estimate of where the robot actually traveled, correctly orientated relative to the earth's magnetic field. The blue data will always be orientated towards the 'X' axis of the odom frame.

If you repeat this test (drive the robot in a 4x4 square again), but you change the initial magnetic heading of your robot, I guarantee that the blue data will look exactly the same every single time, and the red data will simply pivot, something like this:

Hopefully this explanation makes sense, it's just one of these quirks of ROS that you have to get used to. And if someone more knowledgeable than me notices a flaw in my explanation please correct me!

---

Originally posted by M@t with karma: 2327 on 2016-12-21

This answer was ACCEPTED on the original site

Post score: 4

---

Original comments

Comment by jerryzhchao on 2016-12-21:
Thanks very much. @M@t So in your robot, if the GPS cannot work well, the trajectory will deviate a lot as well? Did you try @Tom Moore 's test bag in the r_l package? The IMU+ODOM fusion is much better than mine.

Comment by M@t on 2016-12-22:
Yes, if the GPS is off any EKF output that uses it will be off as well. Are you having issues fusing GPS data? I haven't tried Tom's test bag but his setup may be different. If you're having GPS issues, add it to the question or maybe create a new question for it.

Comment by Tom Moore on 2017-01-04:
Just one note: the +Y axis aligns with whatever your heading reference is. If you only fuse angular velocity, then +Y points +90 degrees from your robot's starting orientation. If you have a compass, the direction of "0" heading is determined by that compass.

Comment by Yvonnn on 2017-06-30:
This : "world-fixed reference frames like odom and map are always orientated so that the 'Y' axis faces magnetic north" is in contradiction with the conventions used in this tutorial : http://docs.ros.org/kinetic/api/robot_localization/html/integrating_gps.html

Comment by Tom Moore on 2017-07-20:
Ignoring GPS data for a moment, if you just fused wheel odometry and yaw from an IMU with a magnetometer, the X axis will always face the direction in which your IMU reads 0.

Comment by Tom Moore on 2017-07-20:
If your IMU reads 0 at magnetic north, then you can (a) just fuse yaw velocity in the EKF and (b) give navsat_transform_node the correct offsets for your IMU (i.e., the values that, when added to the yaw from the IMU, produce 0 when facing east).