robot_localization with hector_sim_imu in 3D mode, robot starts flying up
Really a head scratcher here, my /odometry/filtered always end up with a z velocity and my robot just starts floating upwards. I am using a wheel odom and an imu only. If I disable the imu, the wheel odom works ok.
I am using the hector sim imu on an ENU frame_id:
<plugin name="imu_controller" filename="libhector_gazebo_ros_imu.so">
<robotNamespace>/</robotNamespace>
<updateRate>50.0</updateRate>
<bodyName>imu_link_sim</bodyName>
<topicName>vectornav/IMU</topicName>
<accelDrift>0.005 0.005 0.005</accelDrift>
<accelGaussianNoise>0.005 0.005 0.005</accelGaussianNoise>
<rateDrift>0.005 0.005 0.005 </rateDrift>
<rateGaussianNoise>0.005 0.005 0.005 </rateGaussianNoise>
<headingDrift>0.005</headingDrift>
<headingGaussianNoise>0.005</headingGaussianNoise>
</plugin>
In RVIZ, I double check that the imu_link_sim axis is as follows:
RED: East Green: North Blue: Up
Here is my config:
initial_state: [0.0, 0.0, 0.59,
0.0, 0.0, 0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0]
frequency: 60
sensor_timeout: 0.1
two_d_mode: false
transform_time_offset: 0.0
# Use this parameter to provide specify how long the tf listener should wait for a transform to become available.
# Defaults to 0.0 if unspecified.
transform_timeout: 0.0
# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
publish_tf: true
# Whether to publish the acceleration state. Defaults to false if unspecified.
publish_acceleration: false
map_frame: map # Defaults to "map" if unspecified
odom_frame: odom # Defaults to "odom" if unspecified
base_link_frame: base_link # Defaults to "base_link" if unspecified
world_frame: odom # Defaults to the value of odom_frame if unspecified
odom0: wheel_odom
odom0_config: [true, true, false,
false, false, true,
false, false, false,
false, false, true,
false, false, false]
odom0_queue_size: 5
odom0_differential: false
odom0_relative: false
#
imu0: vectornav/IMU
#The IMU gives orientation, linear acceleration, and angular velocity
imu0_config: [false, false, false,
true, true, true,
false, false, false,
true, true, true,
false, false, false]
imu0_differential: true
imu0_relative: true
imu0_queue_size: 5
# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set
# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame.
imu0_remove_gravitational_acceleration: true
Am I missing something here? This seems to happen as soon as I start moving the robot, not when it first loads.
EDIT, perhaps its because the IMU is so noisy? Here is the plot of the z acceleration when the robot drives forward. Note the spikes happen everytime I move, and recedes as soon as I stop. Do I need to filter the z acceleration somehow?
Does it have to do with the
imu0_remove_gravitational_acceleration: true
? Perhaps you could also include a snippet of the raw data from the imu sensorrostopic echo /robot/imu/imu/data -n1
.it appears that the linear_acceleration/z reported from the imu sensors starts oscillating between 9.4-10.2 once I start to move, and continue to do this even after stopping. This huge discrepancy is the cause, but I've no idea why?