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
- 0.0
- 0.0
- 0.0
- 0.0
- 0.001
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.001
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.001
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.001
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 1.0e-06
twist:
twist:
linear:
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0
covariance:
- 0.001
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.001
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.001
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.001
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.001
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 1.0e-06
/zed2i/zed_node/imu/data:
header:
stamp:
sec: 1688561477
nanosec: 94539456
frame_id: zed2i_imu_link
orientation:
x: 0.04747530817985535
y: 0.0217120498418808
z: 0.33702659606933594
w: 0.9400466680526733
orientation_covariance:
- 5.814558282399458e-10
- -3.2944193591010685e-10
- 3.0220212313595557e-11
- -3.294419012791137e-10
- 8.316236808529003e-10
- 2.0890566503682616e-10
- 3.022021664246969e-11
- 2.0890566503682616e-10
- 6.456938962255484e-10
angular_velocity:
x: -0.01820158927202928
y: 0.02127879501425481
z: 0.000792325946237065
angular_velocity_covariance:
- 6.35957531046334e-10
- 0.0
- 0.0
- 0.0
- 6.81844497277238e-10
- 0.0
- 0.0
- -0.0
- 5.81403604702388e-10
linear_acceleration:
x: 0.024594472721219063
y: 1.1347548961639404
z: 9.869213104248047
linear_acceleration_covariance:
- 0.005106136202812195
- 0.0
- 0.0
- 0.0
- 0.00590071314945817
- 0.0
- 0.0
- -0.0
- 0.006188959814608097
gps/fix:
header:
stamp:
sec: 1688561423
nanosec: 145409472
frame_id: gps_frame
status:
status: 2
service: 9
latitude: 48.477501743301964
longitude: -82.00108049977172
altitude: 232.53988211053635
position_covariance:
- 0.00017811922589316964
- -6.650557043030858e-05
- -1.604144199518487e-05
- -6.650557043030858e-05
- 0.0003876265836879611
- -6.546182703459635e-06
- -1.604144199518487e-05
- -6.546182703459635e-06
- 0.0009483876056037843
position_covariance_type: 3