Ask Your Question
1

Robot_Localization: IMU doesn't update position

asked 2015-07-16 14:39:37 -0500

SilvrCenturion gravatar image

updated 2015-07-21 14:53:12 -0500

Hi,

I'm using the robot_localization package with my phidgetspatial 3/3/3. I know I need more sensors than just an IMU for localization, but as I wait for those to be shipped to me I decided to play with the package and the imu. When I run my launch file, it starts up the imu node, the localization node, and a static transform. When I rostopic echo odometry/filtered, I see the correct output. With one exception. The linear twist never changes from 0, 0, 0, and the position in pose never changes from 0, 0, 0. My orientation and angular velocity appear to change appropriately as I move the imu around, but the position and linear velocity are reported as zeros. Please help. I've included relevant parts of my launch file, but I can't upload it. Not enough points.

Edit: Additionally, my imu/data_raw does report linear acceleration

Edit: Updated launch configuration

<launch>

 <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="false"/>

  <param name="map_frame" value="map"/>

  <param name="odom_frame" value="odom"/>

  <param name="base_link_frame" value="base_link"/>

  <param name="world_frame" value="odom"/>

  <param name="transform_time_offset" value="0.0"/>

  <param name="imu0" value="imu/data_raw"/>

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

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

  <param name="imu0_relative" value="false"/>

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

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

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

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

  <rosparam param="process_noise_covariance">[0.05, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0.05, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0.06, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0.03, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0.03, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0.06, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0.025, 0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0.025, 0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0.04, 0,    0,    0,    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,     0,    0,    0.01, 0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.02, 0,    0,    0,
                                              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,    0,    0,    0.01, 0,
                                              0,    0,    0 ...
(more)
edit retag flag offensive close merge delete

Comments

1

Can you post a sample IMU message? Without more information, I'm first suspecting transforms for the acceleration data, but if your orientations and angular velocities are working, then I'm not so sure.

Tom Moore gravatar imageTom Moore ( 2015-07-18 06:54:02 -0500 )edit

Alright, it is now edited with the IMU messages

SilvrCenturion gravatar imageSilvrCenturion ( 2015-07-20 12:22:55 -0500 )edit

You have an invalid orientation quaternion...orientation: x: 0.0 y: 0.0 z: 0.0 w: 0.0 Orientations need to be reported as unit quaternions

Robocop87 gravatar imageRobocop87 ( 2015-07-20 12:32:49 -0500 )edit

Sure, but isn't the static transform I have setting orientation to 0, 0, 0, 1. Does that not fix it? I'm using the Phidget Spatial 3/3/3 and the Phidgets Imu driver states that it does not include orientation, hence the need for the tf.

SilvrCenturion gravatar imageSilvrCenturion ( 2015-07-20 12:58:59 -0500 )edit

I'm not sure if it is your problem or not, most likely not, but the filter is going to try to use the information in your message, not your transform, and thus it will likely screw it up. If it is a static quat then just set it to be so in your message

Robocop87 gravatar imageRobocop87 ( 2015-07-20 16:00:01 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2015-07-21 06:07:37 -0500

Tom Moore gravatar image

updated 2015-07-29 18:28:11 -0500

You may need to post a bag file, but I see some issues with your configuration. It shouldn't result in the behavior you're seeing, but let's fix it anyway and go from there.

First, your IMU doesn't report orientation at all, but you are fusing roll, pitch, and yaw.

Also, was this IMU in a neutral position, or was it upside-down? On a flat surface and resting right-side up, your IMU should read +Z (+9.81 m/s^2) for acceleration. If you roll it 90 degrees so the left side if facing up, Y should read +9.81. If you pitch it -90 degrees so the front is facing up, it should read +9.81 m/s^2 in X.

Your static transform looks fine to me. It says the IMU is at the vehicle's center and with no rotation. If your IMU is rotated, you'll need to fix that.

Fix these issues and then let's see how it goes.

EDIT after reviewing bag file:

I figured out the problem. When you use accelerations and set the remove_gravitational_acceleration flag to true for that sensor, the filter uses the orientation reported by the IMU to remove acceleration due to gravity. For example, if your IMU was rolled over on its side, we'd need to subtract or add acceleration due to gravity to the Y axis instead of Z. Since your IMU doesn't report orientation, the filter can't compute the opposing vector, and the measurement doesn't get fused.

This leaves you with three options:

  1. If your robot will always be level, just turn off the imu0_remove_gravitational_acceleration flag.
  2. Feed the raw IMU and mag data to imu_filter_madgwick and get a full IMU message, complete with orientation.
  3. Stop using acceleration data.
edit flag offensive delete link more

Comments

I believe I have rectified these issues, and have a bag file, but I do not seem able to attach it. What is the recommended way of providing it?

SilvrCenturion gravatar imageSilvrCenturion ( 2015-07-21 14:56:28 -0500 )edit

Do you have DropBox or Google Drive?

Also, you didn't answer your question about the IMU's orientation.

Tom Moore gravatar imageTom Moore ( 2015-07-24 16:23:37 -0500 )edit

Sorry about that. Here is a google drive link. I believe the orientation is now correct (and was in the bag file) but it was previously incorrect. bag file

SilvrCenturion gravatar imageSilvrCenturion ( 2015-07-27 13:44:07 -0500 )edit

So you fixed those issues, but your problem persists?

Tom Moore gravatar imageTom Moore ( 2015-07-27 17:41:19 -0500 )edit

Correct, fixing the orientation of the device and removing roll, pitch and yaw did not yield non-zero linear velocity or position estimates.

SilvrCenturion gravatar imageSilvrCenturion ( 2015-07-27 19:35:30 -0500 )edit

Fantastic Tom! I'm getting somewhere now. Your help is appreciated.

SilvrCenturion gravatar imageSilvrCenturion ( 2015-07-29 19:01:01 -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

2 followers

Stats

Asked: 2015-07-16 14:39:37 -0500

Seen: 1,245 times

Last updated: Jul 29 '15