robot_localization GPS causes large drift/covariance spike
I am trying to implement the dual ekf navsat example on my real-world robot. The local EKF with wheel odometry and IMU works quite well and has no issues. As far as I can tell, my global EKF configuration is nearly identical to the local with the only addition being the /odometry/gps.
When I start both nodes, the map->odom transform blows up in X/Y and spins randomly when sitting stationary. I have tried adjusting process noise covariance and initial covariance estimate parameters, but these appear to not change the issue.
My GPS appears to be working well. I have RTK Fix and it reports an accuracy of ~2cm, which my testing appears to validate.
Bag file:
I recorded a bag file of the robot sitting still, then drawing a rectangle. The odometry/local appears to draw the shape relatively well. Observing the path of the GPS points also shows relatively low noise, and draws the path well. I can't upload on this site, here is a sharepoint/onedrive link.
Here is a graph showing the /odometry/global (left) vs /odometry/gps and /odometry/local (right):
Details:
- Platform: Nvidia Jetson Xavier
- ROS Version: ROS2 Humble
- Operating System: Ubuntu 22.04 (docker image: arm64v8/ros:humble-perception-jammy)
- Robot_Localization version: 3.5.1-2 (20230525)
- GPS Driver: septentrio-gnss
robot_localization config:
(Process/Initial cov matricies are identical to example)
ekf_filter_node_odom:
ros__parameters:
frequency: 30.0
sensor_timeout: 0.1
two_d_mode: true
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: diff_cont/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: zed2i/zed_node/imu/data
imu0_config: [false, false, false,
true, true, false,
false, false, false,
true, true, true,
true, true, true]
imu0_nodelay: false
imu0_differential: false
imu0_relative: false
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true
use_control: false
ekf_filter_node_map:
ros__parameters:
frequency: 30.0
sensor_timeout: 0.1
two_d_mode: true
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: map
odom0: diff_cont/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: zed2i/zed_node/imu/data
imu0_config: [false, false, false,
true, true, true,
false, false, false,
false, false, false,
false, false, false]
imu0_nodelay: false
imu0_differential: false
imu0_relative: false
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true
odom1: odometry/gps
odom1_config: [true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]
odom1_queue_size: 10
odom1_nodelay: true
odom1_differential: false
odom1_relative: false
use_control: false
navsat_transform:
ros__parameters:
frequency: 30.0
delay: 3.0
magnetic_declination_radians: 0.0
yaw_offset: 0.0
zero_altitude: false
broadcast_utm_transform: true
publish_filtered_gps: true
use_odometry_yaw: false
wait_for_datum: false
Example Sensor Messages:
diff_cont/odom:
header:
stamp:
sec: 1688561428
nanosec: 748273568
frame_id: odom
child_frame_id: base_link
pose:
pose:
position:
x: 0.0
y: 0.0
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
covariance:
- 0.001
- 0.0
- 0.0 ...
x-posted: https://robotics.stackexchange.com/qu...