0
$\begingroup$

Rosanswers logo

Hi all,

I am using Phidgets Spatial 3/3/3 IMU sensor along with rotary encoders to feed to the robot_pose_ekf. For IMU, I am using phidgets_imu and imu_filter_madgwick to get the sensor_msgs/Imu message. The problem is when I start the process and the robot is stationary at its start position, IMU starts drifting and the position of the robot in RVIZ starts changing (the actual robot is stationary). I have attached couple of images showing the change in position of the robot because of drift:

Initial position:

image description

After 7-8 mins (approx):

image description

I dont know whether this is normal or not. This only happens when I add IMU to the input of robot_pose_ekf, if I only have rotary encoders, everything looks OK. Also, I am not using magnetometer in the IMU and here is the relevant part of the code:

<!-- IMU -->
<node pkg="phidgets_imu" type="phidgets_imu_node" name="IMU_node1" output="screen">
    <param name="frame_id" value="base_link"/>
    <param name="period" value="32"/>
</node>
<node pkg="imu_filter_madgwick" type="imu_filter_node" name="IMU_node2" output="screen">
    <param name="use_mag" value="false"/>
    <param name="publish_tf" value="false"/>
    <param name="fixed_frame" value="odom_combined"/>
    <remap from="/imu/data" to="/imu_data"/>    
</node>

Does anyone has any insights on why is this happening and how can it be resolved? Please let me know if you need more information from me.

Thanks in advance.
Naman Kumar

Originally posted by Naman on ROS Answers with karma: 1464 on 2015-05-15

Post score: 0

Original comments

Comment by jxl on 2017-01-18:
@Naman，did you fix your problem，i have a similar question here

Improve this question
$\endgroup$

3 Answers 3

Reset to default
0
$\begingroup$

Rosanswers logo

I've had the same issue of yaw drift with this IMU (also with 'use_mag' set to false) and never had much of a chance to dig into it. All IMUs drift over time, but I'm surprised how quickly this one starts to drift even after calibration. Unfortunately robot_pose_ekf doesn't take /cmd_vel as an input to see that the robot is not being commanded to move (rather it depends solely on wheel odometry for that through topic /odom and the IMU /imu) so the ekf doesn't really help.

So far I've had two..scratch that...THREE hacky solutions to solve this problem which are suboptimal:

  1. Repetitive calibration: Call rosservice /imu/calibrate which will stop the drifting temporarily. Do this whenever the robot is still (very important!) and on flat ground. I had the robot call this service automatically when it detects drift or after a certain timeout threshold. This works well enough for my use case to get the job done....but slight drift will still happen over time regardless. If you're performing SLAM or static map localization (e.g. AMCL), the robot will correct its position in the map frame anyway automatically....so doesn't matter if odom is a little wacky.

OR

  1. Ignore the yaw if you have another sensor measuring yaw: On the base I'm using, there's a very good single-axis gyro which is fused into the wheel odometry by its ROS driver. I would prefer having that yaw versus the drifting one generated through the IMU + robot_pose_ekf -- though I would still like to keep the roll and pitch components from the phidgets. You can either adjust the imu driver code or write a filter node for /imu to change the covariance matrix so the error is high for Z-axis (which corresponds to the yaw component) in the phidgets IMU so that robot_pose_ekf will weight the yaw from the wheel odometry (with the good gyro) more heavily. The other option which doesn't require changing IMU stuff is to write a node that subscribes to both the wheel odometry (/odom) and to the fused result (/odom_combined) that's generated by robot_pose_ekf (which incorporates /odom and /imu). You want to then take the x,y,z,roll,pitch, linV, angV.x, angV.y components from /odom_combined and then take yaw and angV.z from /odom and now publish to a new topic...let's say /odom_final...which is the odometry all your nodes should use. The new node should also publish the transform between /odom and /base_footprint....you should make sure that the same transform from robot_pose_ekf is ignored (you can do this by remapping tf to tf_garbage in robot_pose_ekf).

OR

  1. Just thought of this one and it doesn't require having another gyro. Write a filter node just like in option 2 that takes as input the command velocity (/cmd_vel), the raw wheel odometry (/odom), and the ekf smoothed odometry (/odom_combined). If the latest commanded velocity is 0, you can assume the robot is not moving and that the yaw should be whatever it was when the robot last stopped. If the robot is moving, you can trust the IMU value. Combine this with option 1 where you call the calibrate service whenever the robot is not moving and all should be well. So in summary, when cmd_vel is 0, yaw = lastYaw, cmdVel != 0, yaw = imuYaw. Populate an odometry message with this yaw and the angVel.z along with the other values from /odom_combined excluding yaw and angVel.z and publish them to a new topic /odom_final.

Sorry that #2 and #3 are confusing, but they're the only solutions I could conjure so far. Also note that you'll have to convert r,p,y to quaternion notation for nav_msgs/Odometry. I'm hoping to get more cycles to look into the problem further -- at the very least write the filter I described in step 3. Perhaps Martin can chime in with more suggestions -- can't seem to reference him though so he won't see this message :(

Best of luck!

Originally posted by mirzashah with karma: 1209 on 2015-05-18

This answer was ACCEPTED on the original site

Post score: 3

Improve this answer
$\endgroup$
0
$\begingroup$

Rosanswers logo

Hi Naman, Steven La Valle had an excellent post about drift a while ago. Its pretty interesting and annoying at the same time. link . Take a look at his post. Encountered some drift issues with quadcopters before - some of the fixes can be pretty hacky. But you can zero out your errors by having and environmental truth - some sort of marker in the environment that can aid in drift perception and subsequently correct for it. Or you can have the robot marked out and some sort of external sensing correct for the drift as it perceives the robot deviating.

Originally posted by abishekh with karma: 11 on 2015-05-19

This answer was NOT ACCEPTED on the original site

Post score: 1

Improve this answer
$\endgroup$
0
$\begingroup$

Rosanswers logo

The drift really looks surprisingly high (something like 3°/min?); however, all IMUs drift, and without any external reference (magnetometer), you cannot correct it. And using the magnetometer indoors on a moving robot creates a different set of problems.

Did you try isolating whether the problem is drift from the device or a bug in imu_filter_madgwick? You could plot the yaw angles of the two sensor_msgs/Imu topics and compare.

Finally, are you using localization (amcl/slam_gmapping) at all? Since drift is unavoidable, you'll have to use it anyway at some point, and 3°/min seems like something that it should be able to deal with. Of course, it would still improve the output of localization if you could reduce the drift somehow.

Originally posted by Martin Günther with karma: 11816 on 2015-06-01

This answer was NOT ACCEPTED on the original site

Post score: 2

Original comments

Comment by jxl on 2017-01-18:
@Martin,when i use imu_filter_madgwick to filter imu data ,why the filtered imu data is drifting over ,but the raw imu data does not drift ,link here,did you know why ? Thanks a lot!!

Improve this answer
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.