Robot_Localization: IMU doesn't update position
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 ...
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.
Alright, it is now edited with the IMU messages
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
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.
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