Ask Your Question
1

Yaw problem for IMU fusinon in robot_localization

asked 2016-11-29 01:38:25 -0500

jerryzhchao gravatar image

updated 2016-12-21 05:04:24 -0500

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

Comments

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.

gvdhoorn gravatar imagegvdhoorn ( 2016-11-29 02:25:53 -0500 )edit

@gvdhoorn Thanks for reminder.

jerryzhchao gravatar imagejerryzhchao ( 2016-11-29 02:46:29 -0500 )edit

Can you please also post sample input messages for each of your inputs?

Tom Moore gravatar imageTom Moore ( 2016-12-19 04:08:29 -0500 )edit

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.

M@t gravatar imageM@t ( 2016-12-19 16:03:31 -0500 )edit
1

@M@t Yes the IMU conforms to REP103 with ENU frame. I will prepare a small bag asap.

jerryzhchao gravatar imagejerryzhchao ( 2016-12-19 21:23:10 -0500 )edit

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.

M@t gravatar imageM@t ( 2016-12-19 22:07:57 -0500 )edit

1 Answer

Sort by » oldest newest most voted
4

answered 2016-12-21 15:27:37 -0500

M@t gravatar image

updated 2016-12-21 15:29:22 -0500

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:

image description

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:

image description

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!

edit flag offensive delete link more

Comments

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.

jerryzhchao gravatar imagejerryzhchao ( 2016-12-21 20:57:53 -0500 )edit

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.

M@t gravatar imageM@t ( 2016-12-22 02:54:27 -0500 )edit
2

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.

Tom Moore gravatar imageTom Moore ( 2017-01-04 02:31:37 -0500 )edit

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...

Yvonnn gravatar imageYvonnn ( 2017-06-30 08:20:13 -0500 )edit

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.

Tom Moore gravatar imageTom Moore ( 2017-07-20 04:30:49 -0500 )edit

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).

Tom Moore gravatar imageTom Moore ( 2017-07-20 04:32:17 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2016-11-29 01:38:25 -0500

Seen: 1,559 times

Last updated: Dec 21 '16