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

robot_localization: yaw angle jumping erratically

asked 2014-05-16 04:04:31 -0600

Huibuh gravatar image

updated 2014-05-16 05:51:19 -0600

Hello Tom/All,

I am currently using robot_pose_ekf to fuse wheel odometry and imu data on a custom robot with two tracks. This is wokring pretty well (works perfectly with gmapping and the navigation stack). I am now in the process of adding GPS into the mix, which would allow me to do some outdoor navigation experiments.

As adding GPS as sensor source is not officially supported by robot_pose_ekf, so I was on the lookout for an alternative solution. On paper, I found that robot_localization is exactly what is was looking for. Unfortunately, I am facing issues getting it to work.

My Setup

  1. odom0 is coming from by custom base_controller on topic /wheel_odom as nav_msgs/Odometry. It provides the following data: x, y, yaw, x velocity, yaw velocity. All remaining fields included in nav_msgs/Odometry are 0's.
  2. odom1 is coming from a gps_common/utm_dodmetry_node as nav_msgs/Odometry. It provides the following data: x, y and z. All remaining fields included in nav_msgs/Odometry are 0's.
  3. imu0 is coming from a XSens IMU (lse_xses_mti driver) on topic /imu_data as sensor_msgs/Imu. It provides the following data: roll, pitch and yaw angles. All remaining fields of sensor_msgs/Imu are 0's.

For now, the robot is meant to operate on the plane only. Hence, we can deduce that z is always 0, and so are the roll and pitch angles. My launch file below takes that into account.

My robot_localization launch file

<launch>  
  <!-- Launch robot_localization node -->
  <node pkg="robot_localization" type="ekf_localization_node" name="robot_localization" >
    <remap from="set_pose" to="/robot_localization/set_pose"/>           
    <remap from="odometry/filtered" to="/robot_localization/odom_combined"/>

    <param name="odom_frame" value="odom_combined"/>
    <param name="base_link_frame" value="base_footprint"/>    

    <!-- =============================================================================== -->    
    <!-- Configure odom0 (WHEEL_ODOM) -->
    <param name="odom0" value="/wheel_odom" />
    <rosparam param="odom0_config">[true,  true,  false,    <!-- x, y, z position -->
                                    false, false, true,    <!-- roll, pitch, yaw angles-->
                                    true,  true,  false,   <!-- x/y/z velocity -->
                                    false, false, true]    <!-- roll/pitch/yaw velocity -->
    </rosparam>
    <rosparam param="odom0_differential">[false,  false,  false,   <!-- x, y, z position -->
                                          false,  false,  false]   <!-- roll, pitch, yaw angles-->
    </rosparam>    
    <!-- =============================================================================== -->    
    <!-- Configure odom1 (GPS_ODOM) -->
    <param name="odom1" value="/utm_odometry_node/gps_odom"/>
    <rosparam param="odom1_config">[true,  true,  false,      <!-- x, y, z position -->
                                    false, false, false,   <!-- roll, pitch, yaw angles-->
                                    false, false, false,   <!-- x/y/z velocity -->
                                    false, false, false]   <!-- roll/pitch/yaw velocity -->
    </rosparam>
    <rosparam param="odom1_differential">[false, false, false,  <!-- x, y, z position -->
                                          false, false, false]  <!-- roll, pitch, yaw angles-->
    </rosparam>
    <!-- =============================================================================== -->    
    <!-- Configure imu0 (XSENS)-->
    <param name="imu0" value="/imu_data"/>               
    <rosparam param="imu0_config">[false, false, false,  <!-- x, y, z position -->
                                   false, false, true,   <!-- roll, pitch, yaw angles-->
                                   false, false, false,  <!-- x/y/z velocity -->
                                   false, false, false]  <!-- roll/pitch/yaw velocity -->
    </rosparam>
    <rosparam param="imu0_differential">[false, false, false,   <!-- x, y, z position -->
                                         false, false, false]   <!-- roll, pitch, yaw angles -->
    </rosparam>
    <!-- =============================================================================== -->    
  </node>

</launch>

The Issue

The problem is that the yaw angle of /robot_localization/odom_combined is jumping erratically as long as data is coming in on the /imu_data topic. (yaw on /imu_data is rock solid however).

When I disable the yaw-angle-component of odom0 (/wheel_odom), the problem disappears. I have also ... (more)

edit retag flag offensive close merge delete

Comments

Can you run tf view_frames and include that in your question as well?

ahendrix gravatar image ahendrix  ( 2014-05-16 06:19:36 -0600 )edit

Also, can you verify that you get the transform failure for all measurements, or is it just the first one or two?

Tom Moore gravatar image Tom Moore  ( 2014-05-16 06:22:32 -0600 )edit

When looking at the log file of 10 seconds of operation, I maybe get 3-5 transform failures. So I suppose only some tf's fail.

Huibuh gravatar image Huibuh  ( 2014-05-16 06:45:47 -0600 )edit
add a comment

1 Answer

Sort by ยป oldest newest most voted
3

answered 2014-05-16 06:12:44 -0600

Tom Moore gravatar image

updated 2014-05-17 08:31:18 -0600

So your odom0 topic is providing yaw and yaw velocity? My guess is that whatever is producing that message is integrating the yaw velocities to produce a yaw estimate. robot_localization is already handling that integration, so you should remove yaw from your odom0 measurement. My guess is that what's happening is your odometry's yaw measurement starts to disagree strongly with your IMU's yaw measurement after a few turns, and so you'll see oscillations back and forth as the measurements come in.

EDIT: I see you've already tried excluding yaw from odom0 and it worked. This leads me to believe what I said is true.

A second possibility (and one that I don't really recommend) would be for you to make sure that your yaw variance value for odom0 is growing as the robot makes more turns. Unlike the IMU, the error on your odometry's yaw estimate should grow every time you get nonzero yaw velocity. The IMU, on the other hand, will have a more or less fixed error on its yaw measurements. Eventually, odom0's yaw variance will be so poor that it will be effectively ignored by the filter (though this means there isn't much value in including it anyway).

SECOND EDIT: @Huibuh was kind enough to send me a bag file, and I have two things to add:

To stop the oscillation when the robot first starts, you should change the launch by making the yaw measurements differentially integrated:

<!-- Configure imu0 (XSENS)-->
<param name="imu0" value="/imu_data"/>               
<rosparam param="imu0_config">[false, false, false,  <!-- x, y, z position -->
                               false, false, true,   <!-- roll, pitch, yaw angles-->
                               false, false, false,  <!-- x/y/z velocity -->
                               false, false, false]  <!-- roll/pitch/yaw velocity -->
</rosparam>
<rosparam param="imu0_differential">[false, false, false,   <!-- x, y, z position -->
                                     false, false, true]   <!-- THIS CHANGED! Yaw is now true -->
</rosparam>

Since the robot's yaw measurement from odometry starts at 0, we don't have to set it to true there (though it wouldn't hurt). However, the IMU will start at a non-zero heading. robot_localization allows you to integrate measurements differentially, so the first measurement becomes a "zero point" and subsequent measurements are made relative to that first measurement. As I recall, this is robot_pose_ekf's default behavior (someone correct me if I'm wrong), which is why you didn't see the issue when you used it.

However, my earlier point still stands: as the robot moves, the yaw odometry will start to disagree with the IMU, even if they both start out at 0. For that reason, I'd stick with absolute yaw from the IMU only, and yaw velocity from both the IMU (if you have it) and odometry.

There are other suggestions I'd make for your launch file, but the gist of them are covered in robot_localization's best practices tutorial.

edit flag offensive delete link more

Comments

It makes sense what you are saying. If I set the yaw parameter for odom0 to false, there are no oscillations any more. However, my IMU does not have an absolute reference for yaw (I cannot use the included mag sensors as they won't work next to the big electric drive motors of the robot/won't work reliably indoors), the IMU yaw value will drift over time. By also including yaw from odom0 as measurement source for robot_localization, I was hoping to counteract the IMU gyro drift at least while the robot is stationary.

Huibuh gravatar image Huibuh  ( 2014-05-16 06:43:55 -0600 )edit

Does your IMU produce yaw velocity? If it does, then in your case, I would integrate yaw velocity from both the IMU and odometry, and then exclude all yaw measurements from all sensors. If your absolute yaw measurement can't be trusted, then your best bet is to integrate velocities.

Tom Moore gravatar image Tom Moore  ( 2014-05-16 07:00:48 -0600 )edit

Also, if you have access to the node that generates the odometry, you could try to change the variance on your yaw velocity to a very (very) small number whenever the robot is stationary. This is clearly not ideal, but in this case, it could aid in the drift.

Tom Moore gravatar image Tom Moore  ( 2014-05-16 07:03:04 -0600 )edit
add a comment

Question Tools

1 follower

subscribe to rss feed

Stats

Asked: 2014-05-16 04:04:31 -0600

Seen: 2,768 times

Last updated: May 17 '14

Related questions

wheel encoders, robot_localization and robot_pose_ekf

what IMUs work with ROS?

How do I compute the covariance matrix for an orientation sensor? [closed]

Covariance/filter errors when launching robot_pose_ekf + iRobot Create

What frame_id to put in a sensor_msgs/Imu message?

frames and tf with robot_pose_ekf

What is the robot_pose_ekf current state?

Interpretation of covariance of output of robot_pose_ekf

Using robot_pose_ekf output for Navigation stack

pr2, reset odom_combined

ROS Answers is licensed under Creative Commons Attribution 3.0 Content on this site is licensed under a Creative Commons Attribution Share Alike 3.0 license.
about | faq | help | privacy policy | terms of service
Powered by Askbot version 0.10.2