Yaw shifting on not moving outdoor robot with robot localization
I am using robot_localization package for the localization of a robot placed outdoors and equipped with RTK GPS, wheel odometry and IMU.
When the robot is placed somewhere and is not moving, I can notice that there is a drift in the yaw output of the filtered odometry.
I am using one instance for odom->base_link transform, only with wheel odometry and IMU. The map-> base_link transform is using wheel odometry, IMU and the GPS.
Why is this happening?
Also, when the robot starts moving we notice in Rviz that the robot performs a full rotation around itself before moving forward, while the raw IMU measurements do not show any such rotation.
Below is the configuration that I use with the robot_localization for odom->base link.
frequency: 30
sensor_timeout: 0.1
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.2
print_diagnostics: true
debug: false
publish_tf: true
map_frame: map
odom_frame: robot_odom
base_link_frame: robot_base_footprint
world_frame: robot_odom
# Wheel odometry:
odom0: wheel_odom
odom0_config: [false, false, false,
false, false, false,
true, true, false,
false, false, false,
false, false, false]
odom0_queue_size: 10
odom0_nodelay: false
odom0_differential: true
odom0_relative: false
# imu configure:
imu0: imu/data
imu0_config: [false, false, false,
false, false, true,
false, false, false,
false, false, true,
false, false, false]
imu0_nodelay: true
imu0_differential: true
imu0_relative: false
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true
gravitational_acceleration: 9.7530
initial_estimate_covariance: [1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0]
For map->odom transformation, the configuration is below.
frequency: 20
sensor_timeout: 0.1
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.2
print_diagnostics: true
debug: false
publish_tf: true
map_frame: map
odom_frame: robot_odom
base_link_frame: robot_base_footprint
world_frame: map
# Wheel odometry:
odom0: wheel_odom
odom0_config: [false, false, false,
false, false, false,
true, true, false ...
Hi, Have you found any solution? I also have issues with a similar configuration. The issue I am facing is mainly the same (robot drifting when it is stopped) but there is also another major problem. When the node starts and the robot starts moving the
odometry/filteredrotates and jumps around for a while until it moves for about 3 meters. Afterward, the localization works fine until it stops and the drift you mentioned is presented again.Have you tried looking at the raw IMU data? Is it also drifting? The EKF won't be able to correct for erroneous data.
Please add a sample message from each sensor input.
I added a sample message from each sensor input above.
By looking the raw IMU data from the robot, we notice that there is a small drift in the measuments, so you're right this must be the cause for drifting while not moving. But when the robot starts moving, the IMU measurement has normal values, but the robot in Rviz rotates around itself before moving straight. Is this somehow related to the IMU?
I think @tomarRobin's answer is correct, but to confirm, you can just turn off inputs. First, make sure you enable yaw velocity from the wheel encoders (and leave that on; there's no reason to have it disabled). Next, try not fusing the IMU at all. If the EKF output looks OK, throw in yaw velocity _only_ from the IMU. If it still looks OK, then you obviously have an issue with the magnetometer data (i.e., the yaw) from the IMU. But my guess is that tomarRobin's thoughts will be accurate, and your angular velocity from the IMU has nonzero values when stationary (this could be in addition to the mag data being bad). You can modify the IMU driver to do what was suggested in the answer below or you can write an interim node, as suggested.