robot_localization transform from map to odom drifts
Hi, I am trying to set up robot_localization on an outdoor mobile robot that I am building from scratch. The vehicle is a four wheeled mobile robot and the onboard computer is a Raspberry Pi 3 with a Navio2 hat from Emlid. The Navio2 has two IMUs: a MPU9250 and a LSM9DS1 and a U-blox M8N GPS (among other sensors). I developed a navio2_ros package to access those sensors data as ROS nodes. In my setup I am currently using the odometry from the DC motors, the LSM9DS1 and the GPS.
I have properly configured the IMU following the documentation and calibrated the magnetometer using the robot_calibration package. The gyroscope and accelerometer bias are removed with the imu_complementary_filter package and then imu_filter_madgwick package provides the orientation in ENU using the magnetometer. When the vehicle faces East, the yaw angle is 0 and when the vehicle faces North, the yaw angle is 90 degrees.
Then I have tried to configure a dual EKF setup with two ekf_localization_node and a navsat_transform_node. My launch file goes like this
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_odom" clear_params="true" output="screen">
<remap from="odometry/filtered" to="odometry/filtered_odom"/>
</node>
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_map" clear_params="true" output="screen">
<remap from="odometry/filtered" to="odometry/filtered_map"/>
</node>
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" clear_params="true" output="screen">
<remap from="odometry/filtered" to="odometry/filtered_map"/>
<remap from="gps/fix" to="/gps/fix" />
</node>
and the YAML configuration file has the following content
ekf_se_odom:
frequency: 20
sensor_timeout: 0.1
two_d_mode: false
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom
odom0: hibachi_velocity_controller/odom
odom0_config: [false, false, false,
false, false, false,
true, true, true,
false, false, true,
false, false, false]
odom0_queue_size: 10
odom0_nodelay: true
odom0_differential: false
odom0_relative: false
imu0: imu/data
imu0_config: [false, false, false, # x, y, z
true, true, true, # r, p, w (yaw)
false, false, false, # dx, dy, dz
true, true, true, # dr, dp, dw
false, false, false] # ddx, ddy, ddz
imu0_nodelay: true
imu0_differential: true
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true
use_control: false
ekf_se_map:
frequency: 20
sensor_timeout: 0.1
two_d_mode: false
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
dynamic_process_noise_covariance: false
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: map
odom0: hibachi_velocity_controller/odom
odom0_config: [false, false, false, # x, y, z
false, false, false, # r, p, w (yaw)
true, true, true, # dx, dy, dz
false, false, true, # dr, dp, dw
false, false, false] # ddx, ddy, ddz
odom0_queue_size: 10
odom0_nodelay: true
odom0_differential: false
odom0_relative: false
odom1: odometry/gps
odom1_config: [true, true, false, # x, y, z
false, false, false, # r, p, w (yaw)
false, false, false, # dx, dy, dz
false, false, false, # dr, dp, dw
false, false, false] # ddx, ddy, ddz
odom1_queue_size: 10
odom1_nodelay: true
odom1_differential: false
odom1_relative: false
imu0: imu/data
imu0_config: [false, false, false, # x, y, z
true, true, true, # r, p, w (yaw)
false, false, false, # dx, dy, dz
true, true, true, # dr, dp, dw
false ...
Can you try NOT using the IMU data for both the odom and map EKFs? IMU's have such a high update rate that they can rapidly introduce error into the estimate.
@JakB thanks for the response! I removed the IMU from the local EKF but the results are fairly the same. Since the dual EKF and navsat example from the robot_localization package uses IMU in both filters, could you explain me a little bit more about that rationale?
@gmsanchez can you remove it from both the local and map filters please and try it? The reason is that IMUs usually produce data at a very fast rate and also can perform poorly when they are not under going large accelerations which ground robots rarely do. For both of these reasons they can add a lot of error to the state estimate quickly. Also interference from surrounding metal and electrical motors can interfere with the magnetic measurements as well.
@JackB in that case I should set "use_odometry_yaw" in navsat_transform node to true, right?
@gmsanchez no. The navsat transform node uses the IMU separately from the filter nodes so you shouldnt need to change anything there.
@JackB thanks for the responses. I have tried to do what you suggested. The map <--> odom transform drifts. You can see what happens in the following screen captures of RViz
is that expected behavior?
Thanks in advance!
It looks like your GPS is so noisy that it will be hard to generate a reliable origin from that?
@JackB I am using a pair of Emlid Reach GPS and using reach_ros_node to provide the data to robot_localization. I have configured one of them as a base station and the other as a rover. The base station sends the latitude, longitude and altitude corrections to the rover and as far as the configuration web page tells, the rover has centimeter level accuracy.