Integrate an IMU sensor with robot_localization

Question

Hello,

I'm trying to integrate an IMU sensor to my mobile robot no holonomic.

I follow the robot_localization tutorial to do that, but I'm a little confused with some questions.

• First, how should be my resulting tf tree? I think the frame "odom_ekf" provided from ekf_localization node would be at the top of the tree. The base_link frame would be down. So when I called the set_pose service provided from ekf_localization node, this service could changed the values of transformed between "odom_ekf" frame and "base_link" frame. Is that correct?

• Second, only starting to fuse the IMU sensor with the odometry, how the launch file of robot_localization should be?

    <launch>
    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true" output="screen">
  
      <param name="frequency" value="30"/>
      <param name="sensor_timeout" value="0.1"/>
  
      <param name="odom_frame" value="odom_ekf"/>
      <param name="base_link_frame" value="base_link"/>
      <param name="world_frame" value="odom_ekf"/>
  
      <param name="odom0" value="/myOdomTopic"/>
      <param name="imu0" value="/myImuTopic"/>
      <rosparam param="odom0_config">[false, false, true,
                                      true, true, true,
                                      true, true,true,
                                      true, true, true,
                                      true, true, true]</rosparam>
  
      <rosparam param="imu0_config">[false, false, false,
                                     true,  true,  true,
                                     false, false, false,
                                     false, false, false,
                                     true,  true,  true]</rosparam>
  
      <param name="imu0_remove_gravitational_acceleration" value="true"/>
      <param name="odom0_remove_gravitational_acceleration" value="true"/>
  
      <param name="odom0_differential" value="true"/>
      <param name="imu0_differential" value="false"/>
      <remap from="/odometry/filtered" to="/odometry/odom_imu" />
  
     </node>
    </launch>
  

With this launch, the tf between odom_ekf and base_link is published and the topic /odometry/filtered shows correctly the robot position. But:

• The topic /odometry/filtered doesn't take into a count the changes at orientation. The position is more or less fine, but the orientation doesn't change.
• The remap to /odometry/odom_imu is not working.
• When the robot_localization is running these warnings appears constantly:

"[ WARN] [1417095211.082070235]: MessageFilter [target=odom_ekf ]: Dropped 100.00% of messages so far. Please turn the [ros.robot_localization.message_notifier] rosconsole logger to DEBUG for more information. [ WARN] [1417095211.082749350]: MessageFilter [target=base_link ]: Dropped 100.00% of messages so far. Please turn the [ros.robot_localization.message_notifier] rosconsole logger to DEBUG for more information."

I think first I have to solve this problems and then integrate the GPS sensor.

I'm not sure what I'm doing wrong. Some help would be greatly appreciated.

UPDATE 1:

Here is my frames.pdf

I've corrected some errors with the tf and now the robot_localization node doesn't show any errors.

The problem now is that the odometry/filetered isn't given the correct position and orientation. This is my launch file:

<launch>
  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true" output="screen">
<param name="frequency" value="30"/>
<param name="sensor_timeout" value="0.1"/>
  <param name="odom_frame" value="summit_a/odom"/>
      <param name="base_link_frame" value="summit_a/base_footprint"/>
<param name="world_frame" value="summit_a/odom"/>
<param name="odom0" value="summit_xl_controller/odom"/>
 <rosparam param="odom0_config">[true, true, false,
                                      false, false, true,
                                      true, true, false,
                                      false, false, true,
                                      false, false, false]
                                      </rosparam>
<rosparam param="imu0_config">[false, false, false,
                                     false, false, false,
                                     false, false, false,
                                     false, false, true,
                                     false, false, false]</rosparam>
  <param name="two_d_mode" value="true"/>
      <param name="odom0_differential" value="true"/>
<param name="imu0_differential" value="true"/>
   </node>
</launch>

The odometry of odom0 is really good, here is an example of the topic publication:

---
header: 
  seq: 209396
  stamp: 
    secs: 1417780933
    nsecs: 699532957
  frame_id: summit_a/odom
child_frame_id: summit_a/base_footprint
pose: 
  pose: 
    position: 
      x: -0.440918549019
      y: -0.0679257786528
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: -0.568167798045
      w: 0.822912725181
  covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0]
twist: 
  twist: 
    linear: 
      x: 0.0
      y: -0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0]

---

The odometry filtered isn't working good. If the robot doesn't move, this topic shows the orientation always turning. Here is an example of the topic publication:

---
header: 
  seq: 8992
  stamp: 
    secs: 1417780819
    nsecs: 477535619
  frame_id: summit_a/odom
child_frame_id: summit_a/base_footprint
pose: 
  pose: 
    position: 
      x: -0.83724089347
      y: -0.0834189954256
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.117655236114
      w: 0.993054502741
  covariance: [0.0029236240180075704, 5.293955920339622e-23, 0.0, 0.0, 0.0, 6.017863108528476e-18, -1.0587911840678905e-22, 0.0029236240180075704, 0.0, 0.0, 0.0, -4.7450567233553104e-17, 0.0, 0.0, 4.999843715844642e-07, 1.3588785312409286e-34, 1.2380407640166175e-33, 0.0, 0.0, 0.0, 1.3588785312409286e-34, 4.997917817531666e-07, -1.0764669684467374e-56, 0.0, 0.0, 0.0, 1.238040764016617e-33, -1.0764669684467372e-56, 4.997917817531666e-07, 0.0, 6.017863108528477e-18, -4.745056723355312e-17, 0.0, 0.0, 0.0, 103.94554202745518]
twist: 
  twist: 
    linear: 
      x: -4.75634547929e-18
      y: 5.23875168445e-19
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.00553798395805
  covariance: [0.002695781736480738, 2.0610478214533238e-23, 0.0, 0.0, 0.0, 9.98187210629622e-21, 1.8738127076150572e-22, 0.002695781736480738, 0.0, 0.0, 0.0, 9.215002493653128e-20, 0.0, 0.0, 4.998750273990059e-07, 3.3137790604351043e-43, 3.019102511433211e-42, 0.0, 0.0, 0.0, 3.313779060435105e-43, 4.969126041719983e-07, -6.40125996972557e-64, 0.0, 0.0, 0.0, 3.019102511433213e-42, -6.401259969725568e-64, 4.969126041719983e-07, 0.0, 9.981872106296216e-21, 9.21500249365313e-20, 0.0, 0.0, 0.0, 0.15665066076132653]
---

My IMU only offers relative parameters of angular velocity and linear acceleration:

---
header: 
  seq: 33213
  stamp: 
    secs: 1417785413
    nsecs: 767263020
  frame_id: summit_a/imu
orientation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 1.0
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: -0.00261799398202
  y: -0.00418879011116
  z: 0.0059341195192
angular_velocity_covariance: [0.005235987755982988, 0.0, 0.0, 0.0, 0.005235987755982988, 0.0, 0.0, 0.0, 0.005235987755982988]
linear_acceleration: 
  x: -0.165410732422
  y: 0.134250646122
  z: 9.80570288122
linear_acceleration_covariance: [0.19454589267577976, 0.0, 0.0, 0.0, 0.194857493538782, 0.0, 0.0, 0.0, 0.09814297118782998]

Here is an example of a movement in rviz. The red arroys are the original odometry, and the green arroys are the odometry_filtered returned by robot_localization.

UPDATE 2:

Sorry, I forgot copy the line of <param name="imu0" value="topic_of_your_imu"/>

I've corrected the orientatión data. Now it works better, but the filtered pose estimation isn't better than just the odometry.

I've tested it with the absolute values of the odom:

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

The imu0 config is:

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

Here is the new test in rviz. I've recorded it in a bag file.

I've also tested it with the velocity of the odom0:

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

And the same config for the imu as before.

Here is the rviz capture. And here is the bagfile.

UPDATE 3: (Tom thank you very much for your quick replies.)

My Summit isn't holonomic. It has normal wheels.

I've corrected the covariances and the velocities reported but the results are similar. I don't see any problem with the time stamps. All is running in the same machine.

Here is my last launch file for the absolute estimation, a bagfile and a capture of the results.

Here is my last launch file for the velocities, the bagfile and a capture of the results.

Just the odometry (red color) is better than the odometry filtered with IMU (green color).

UPDATE 4:

I've downloaded the last Github version of robot_localization and the results are the same. I've used your launch file and my output in rviz is ok, the same than yours. I've verified that the delay appears when the yaw velocity of the raw odometry is set to false at the input of the filter. So the main problem is that the filter can't estimate fine the yaw position or velocity without this input. The point is to know if the IMU is the reason.

The IMU doesn't have any internal clock. Its timestamp is set by "ros::time::now();". However, I don't find anything wrong with its tf. I've turned debug on but any error message appears to me. I've no idea why it shows them to you. Here is my debug.txt of robot_localization.(if it helps)

I've worked with my rosbag of velocities (This one) in order to see the same delay that you show me in your plots.

I've obtained the raw odometry messages from the Summit odom topic. raw_odometry.txt

I've obtained the odometry filtered of the new robot_localization node launched your new launch with v_yaw to true at the input of the filter.new_odometry_filtered.txt

I've obtained the odometry filtered by my rosbag (with yaw velocity of odom0 to false): imu_odometry.txt

This is the script for the octave (some comands can change in Matlab) that I used and these are some functions needed by the script. (qGetR.m,r2rpy.m)

The script shows 2 plots:

First plot is like yours third but It doesn't show any delay. The raw velocity measurements from odometry (red), the yaw velocity estimated by the positions (blue) and the yaw velocity for the new odometry_filtered (green). Note that this odometry_filtered is not from my rosbag, is from the new robot_localization of your launch file. There's no delay when the input of the filter has the yaw velocity of the raw odometry (odom0) set to true.

The second plot shows the delay between the yaw velocity of raw odometry (red) and the yaw velocity of the odometry_filtered(blue) when the input filter has the yaw velocity of the raw odometry set to false. Note that here I've used the odometry_filtered from my rosbag robot localization.(odom_imu topic)

Please, what I am doing wrong? How do you show the delay between the yaw velocity of the odometry and the yaw velocity estimated? Any ideas or suggestions would be appreciated.

UPDATE 5

Tom thank you very much, your efforts are really appreciated. Thanks to you I found what's going wrong. You're right. Summit's calculating the yaw velocity based on the movement of the wheels but taking the yaw orientation from a gyro. Obviously, the integration of the raw yaw velocity couldn't match with the raw yaw position.

I've modified the code and now here is the raw yaw position vs yaw position estimated by yaw velocity:

And here is the raw yaw velocity vs yaw position estimated by yaw position.

They fix perfectly. Zoom in:

(in response to your edit 5)

I drove a square. Here's the rosbag file (just with the ekf_localization topics). And here is my launch file. This is a capture of the test from rviz:

Everything works for me. The odometry filtered is better than just the raw odometry

Thanks a lot, Tom.

If you need anything and I can help you feel free to ask me.

Now, I'll try to integrate the GPS sensor to robot_localization node.

Best regards and thanks again.

---

Originally posted by ASoriano on ROS Answers with karma: 95 on 2014-11-27

Post score: 4

---

Original comments

Comment by Tom Moore on 2014-12-09:
Question before I update the answer: is this robot holonomic? It doesn't look it, but I know the Summit XLs can have omnidirectional wheels.

Comment by Tom Moore on 2014-12-15:
Comment in response to update 3: I'll take a look at this. Just so I'm clear, for the path that you drove, the robot's real-world start and end locations were exactly the same, correct?

Comment by Tom Moore on 2014-12-15:
One more thing: I keep getting errors when I attempt to filter your ROS bag files. This happened for the last file as well. The error is long, but here's the last line of it:

UnicodeDecodeError: 'ascii' codec can't decode byte 0xc2 in position 4080: ordinal not in range(128)

Comment by ASoriano on 2014-12-15:
The path that I drove finished more or less at the same location that started. Just the odometry (red) is so reliable, the odometry_filtered (green) should be very similiar to odometry(red).

Comment by ASoriano on 2014-12-15:
I don't have any problem playing my bagfiles. I'm using hydro, maybe you're using indigo and something at bagfiles changed?

Comment by Tom Moore on 2014-12-22:
Great! I'm glad it's working for you. Do you mind if I use your bag file for generating new integration tests? I ask because it would likely become part of the package, at least until I locate some web space for storing them.

Comment by ASoriano on 2014-12-22:
Of course. Feel free to use my bag file.

Comment by novak on 2017-03-29:
ASoriano, Do you have the file where you have fill the imu msg? I use rosserial_arduino to do that but I want to compare other methods. Thank you.

Comment by ASoriano on 2017-11-02:
No, I have not. Sorry. This was 3 years ago.

Answer 1

Before I start, it would be super helpful if you would also post sample messages from your odometry and IMU. Also, if you can, maybe post your tf tree?

To your questions:

• If your world_frame is the same as your odom_frame (which is the case for you), then the node outputs a transform from your world_frame to your odom_frame. This transform is the same as the position that is output by the filter, so yes, if you call the set_pose service, the transform should also change.

• Does your IMU provide only absolute orientation, or does it provide rotational velocity as well? Right now, you have it set for absolute orientation and linear acceleration only. If you wanted rotational velocity as well, do this:

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

Unless you have good reason, set the imu0_differential setting to true. Will you be starting on an incline?

• The warning is very telling. The issue is that the filter can't transform your IMU messages into the target tf frame. If you post your tf tree and a sample IMU message, that will help.

• Side note 1: gravitational acceleration only applies to IMU topics. You can remove

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

• Side note 2: Your odometry configuration needs some tweaking. Is your robot operating in 2D or 3D? If it's 3D (or if you want to include subtle variations in the ground plane for a robot operating in 2D), then does your odometry data actually fill out roll, pitch, roll velocity, and pitch velocity? Also, your odometry data doesn't contain linear acceleration, so the last three true values are unnecessary. I'd have to see your odometry data to give you a more definitive answer, but for now, try this:

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

Fix those issues and post your messages and I can help more.

EDIT after update:

Two things:

• Looking at your rviz output, it seems as though one of your orientation data sources is producing headings (or heading velocities) that are the wrong sign. As per this page on the wiki, it's critical that you look at the output of each of your sensors, one at a time, and make sure that the signs are correct and in adherence with REP-103. If your robot turns left, that is positive yaw. You'll have to covert from quaternions in the messages to Euler angles (unless you're good at deciphering quaternions).

• For your odometry data, use the velocity or use the absolute position. Don't use both. If your odometry source is generating body-frame velocities (and make sure they're in the body frame), then I'd go with those, and set X, Y, and yaw to false.

Fix those, and then let me know how it goes. If it's still giving you trouble, shoot me a bag file and I can update this question.

EDIT 2: actually, looking at your launch file, I don't see you actually including your IMU at all. You need this:

   <param name="imu0" value="topic_of_your_imu"/>

EDIT 3 (in response to update 2):

You have a few culprits:

1. First, your covariances. See these pages:

   http://wiki.ros.org/robot_localization/Troubleshooting http://wiki.ros.org/robot_localization/Tutorials/Preparing%20Your%20Sensor%20Data

   Here is your IMU's angular velocity:

            angular_velocity: 
              x: 0.00209439505558
              y: -0.00191986216679
              z: 1.14231794217
           angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
   

You have zeros for roll, pitch, and yaw velocity variance. Set them to realistic values.

Here is your odometry pose data:

               pose: 
                   position: 
                     x: -0.18140229997
                     y: 1.41788893847
                     z: 0.0
                   orientation: 
                     x: 0.0
                     y: 0.0
                     z: -0.956499625695
                     w: 0.291733553171
                 covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0]

...and here is your odometry twist data:

               twist: 
                 linear: 
                   x: 0.442822994916
                   y: -0.298867051238
                   z: 0.0
                 angular: 
                   x: 0.0
                   y: 0.0
                   z: -0.83410439336
                 covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0]

For your "absolute" configuration, you have X, Y, yaw, and yaw velocity set to true. However, both your yaw and yaw velocity covariances are set to 99999. A covariance of 99999 means "this measurement has a huge amount of error." The filter is going to effectively ignore it. Also, your X and Y covariances are tiny. The same is true for your velocity covariances and your "velocity" configuration.

2. There may be another issue with your velocity data, and it's almost certainly the reason your velocity configuration produced strange results. In your odometry data, you have the child_frame_id set to summit_a/base_footprint, which a coordinate frame that is affixed to your robot. Therefore, unless your robot is holonomic (and I realize it may be, as some of the Summits have omnidirectional wheels), it should not have Y velocity. If you look at the data above, you have Y velocities being reported. It seems to me that your velocity is being reported in the world frame (i.e., the frame_id of your message), and not the body frame (the child_frame_id). Is that the default velocity data coming from the Summit XL? If so, that should probably be fixed. I feel like this is the second time I've seen that problem.

3. Finally, your IMU data is being rejected by the filter on my end. It's saying that it doesn't like the time stamp on it. Is all the data coming from the same machine, and if not, are all the machines' clocks synchronized?

So: fix your covariances. Then, if you want to try the absolute version again, give it a shot. If you'd rather work with the velocity data, then fix it so it's actually reported in the body frame (assuming it's not holonomic), and then try that again. Thanks for the bags; they make answering these questions much easier. If you have a further update, please also post the full launch file that you used (as a DropBox link; it's going to get messy if we keep posting them in full here).

EDIT 4 (in response to update 3):

Here's the launch file I used. Please ignore/remove anything to do with rosbag or the static_transform_publishers. I included them just so you would see all of my settings.

<launch>

  <node pkg="tf" type="static_transform_publisher" name="a" args="0 0 0.2825 0 0 0 /summit_a/base_link /summit_a/vbox 20" />
  <node pkg="tf" type="static_transform_publisher" name="aa" args="0 0 0.2825 0 0 0 /summit_a/vbox /summit_a/imu 20" />
  <node pkg="tf" type="static_transform_publisher" name="aaa" args="0 0 0.2825 0 0 0 /summit_a/vbox /summit_a/gps 20" />
  <node pkg="tf" type="static_transform_publisher" name="aaaa" args="0.05 0 0.325 0 0 0 /summit_a/base_link /summit_a/hokuyo_laser_link 20" />
  <node pkg="tf" type="static_transform_publisher" name="aaaaa" args="0 0 0 0 0 0 /summit_a/hokuyo_laser_link /summit_a/hokuyo_laser_box_link 20" />
  <node pkg="tf" type="static_transform_publisher" name="aaaaaa" args="0 0 0 0 0 0 /summit_a/base_footprint /summit_a/base_link 20" />

  <node pkg="rosbag" type="play" name="rosbagplay" args="soriano_filtered.bag --clock -d 5 -s 6" required="true"/>

  <group ns="summit_a"> 

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

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

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

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

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

      <param name="odom0" value="summit_xl_controller/odom"/>
      <param name="imu0" value="vbox3i/Imu"/>

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

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

      <param name="odom0_differential" value="false"/>
      <param name="imu0_differential" value="false"/>

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

      <param name="debug"           value="false"/>
      <param name="debug_out_file"  value="debug.txt"/>

    </node>
         
  </group>

</launch>

Second, here's a picture of my output. Yellow is the raw odometry, blue is the what robot_localization (ekf_localization_node) is generating on my machine, and red is what you were getting with your velocity settings (it's just the data in the bag file). As you can see, blue and yellow are right on top of one another, so ekf_localization_node is working for me with the settings I have above. Note that IMU is not being used; more on that in a second.

Note that I am using the latest version in the GitHub repo. I did not run this on the released version, but it shouldn't be any different.

EDIT: Removing these plots, as I've got better ones below, and my hypothesis as to what's wrong with your data has changed as well. See update 6.

For now, I've changed your launch file so that it uses absolute yaw and X velocity from your odometry. I also have set odom0_differential to false, as I don't have the time at the moment to play with the covariances on your absolute yaw data and make it works perfectly with the differential setting. However, as your odometry yaw data starts at 0 and you are only going to use yaw velocity from your IMU, this won't matter for you. If you get an IMU that provides absolute yaw, it will become a problem, though, so be aware.

EDIT 5 (in response to update 4):

OK, I think we may have a disconnect. I can see from your debug text file (and I thank you for providing it) that for you, the IMU data is being accepted. Part of the reason for the tf timing issue I'm having may be that I cannot filter your bag files (I get the error I mentioned previously), so instead I record a new one while I play back the old one, but only record what I want. If we want to sort this out faster, here's what I need (and I plan to put this on the wiki):

1. Drive your robot in a square, and make sure it comes back more or less exactly to the start location. If your driving pattern differs from this, give me in detail what the robot did, and where it ended up. Otherwise, I have no context for what it means when you say something is "worse."
2. When you record the bag file, please record only the input topics to ekf_localization_node. Do not record your tf tree, though you should probably provide the values of any static transforms you're broadcasting.
3. Your launch file.

Thanks!

EDIT 6 (still in response to update 4):

OK, I got some more time to look over your plots and mine, and I believe I've found the issue.

Here's what I did: I played back your Velocities2.bag file that you linked in your question. I then did

rostopic echo /summit_a/summit_xl_controller/odom -p > odom.txt

From the odom.txt file, I imported these columns into Matlab:

Position (X, Y, Z) Orientation (X, Y, Z, W) Angular Velocity (X, Y, Z)

For my first plot, what I want to show is that if you integrate all of your angular velocities over time, you should get more or less the same orientation as your raw orientation data. In other words, we add up all the small changes in yaw that we get at every time step, and it should match the absolute yaw. So I did this in Matlab:

odom_yawvel_integrated = cumsum(odom_yawvel)*0.02; % Your odometry gets reported at 50 Hz 

This effectively integrates your yaw velocity from your odometry data. Now I plot both your yaw and yaw velocity data:

figure(1)
plot(odom_yaw);
hold on;
plot(odom_yawvel_integrated, 'r');
legend('Raw odometry yaw', 'Accumulated yaw from raw yaw velocity');

And this is what I get:

What this plot means is that your yaw velocity is too fast. These plots should overlay pretty much on top of one another. Your raw yaw stops around -3 radians after the first turn, but your yaw velocity keeps going until the accumulated value (which is effectively what the filter is doing) is around -4 radians. There's something not jiving between your odometry's orientation data and its yaw velocity data.

Now let's go the other way: we'll plot the raw yaw velocity data from your odometry, and overlay the calculated velocity that we get by differentiating the absolute yaw:

figure(2)
plot(odom_yawvel);
hold on;
plot(diff(odom_yaw)/0.02, 'r'); % Calculating yaw velocity from absolute yaw, using 50 Hz data rate
legend('Raw odometry yaw velocity', 'Calculated yaw velocity from absolute yaw');

This yields:

It's hard to see, so I zoomed in on the data from the first turn:

The blue values are your raw yaw velocities, and you can see that they are faster than your differentiated yaw data.

Most important, however, is a plot of your raw odometry yaw velocity and your raw IMU yaw velocity:

figure(3);
plot(odom_yawvel_decim); % Still the raw data, but had to decimate so they'd line up in the plot (different frequencies)
hold on;
plot(imu_yawvel, 'r');
legend('Raw Odometry Yaw Velocity', 'Raw IMU Yaw Velocity');

This yields:

Again, you can see how the raw odometry yaw velocity is always "faster" than the raw IMU yaw velocity.

Anyway, what I'm trying to show is that one of the issues for you is that you odometry's yaw velocity data is wrong. However, the last launch file I posted should have worked better for you, as I already disabled yaw velocity for your odom0 source and enabled it for your imu0 source. Did that not help? In any case, if you get time, please record a new bag (as instructed above) with my updated settings.

Oh, one more question: can I see your Summit XL launch files? Looking at this:

https://code.google.com/p/summit-xl-ros-stack/source/browse/trunk/trunk/summit_xl_sim_hydro/summit_xl_robot_control/src/summit_xl_robot_control.cpp

...it appears as if the odometry data is getting its angular velocity and orientation from an IMU, and not from wheel encoders (unless I'm missing something). Perhaps I'm looking at the wrong file?

EDIT 7 (yes, still in response to update 4):

I just plotted your raw odometry and the "new" filtered output (from your last update) on top of one another. Here's what I see:

The two end up ~2 cm apart from one another. As it would be hard to measure that difference in the real world, how are you making the determination that the filter isn't any "better" than the raw odometry? I guess what I want to know is what you were expecting to see from the data. Also, I'm interested to know where your Summit XL is getting its orientation and angular velocity data for its odometry message.

---

Originally posted by Tom Moore with karma: 13689 on 2014-11-27

This answer was ACCEPTED on the original site

Post score: 5