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
- 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: 40.477501743301964
longitude: -87.00108049977172
altitude: 182.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
Asked by brow1633 on 2023-07-05 13:52:36 UTC
Answers
There are a few ideas for you to debug.
Is your IMU reporting the data in the ENU convention? This means when your robot is facing east, it should report zero yaw.
Have you set up the magnetic declination correctly? From experience, IMU often cannot provide a very good estimate of absolute heading. It might cause discrepancies between the heading in the GPS coordinate and the IMU coordinate, which will screw up your EKF estimation. I would prefer to have a GPS compass if possible, and just fuse the angular speed from the IMU.
Is the covariance of your GPS measurement too low? or is the covariance of your local measurement too high? Both cases will lead to the robot trying to "snap" onto the GPS location. Typically, we want the GPS to adjust our local estimation gracefully, which means the GPS measurement covariance should be somewhat of a larger value. You can try to lower your process noise, and reduce the value of the covariance of your wheel odometry.
I hope this is helpful for the problem that you are facing.
Asked by hank880907 on 2023-07-06 07:31:07 UTC
Comments
I have a dual antenna GPS setup - so eventually the plan would be to fuse the heading output from that with angular speed from IMU as you suggest.
Without correct ENU convention and magnetic declination, am I correct in thinking that this will only affect the absolute orientation estimate of the filter? I.E. even if they're both wrong, the filter should work correctly with only IMU orientation fused, albeit with incorrect geo-referenced orientation?
I appreciate the covariance tuning guidance!
Asked by brow1633 on 2023-07-06 07:40:37 UTC
Without the correct ENU convention, you need to disable the orientation of the IMU from fusing into the global EKF. This means only the acceleration and the angular speed are fused.
imu0_config: [
false, false, false,
false, false, false,
false, false, false,
true, true, true,
true, true, true]
Also, you need to enable heading from your GPS if it provides a heading.
odom1: odometry/gps
odom1_config:
[true, true, false,
false, false, true,
false, false, false,
false, false, false,
false, false, false]
If your IMU does not follow the ENU convention, you cannot fuse it into the global EKF. However, it would be fine to fuse it into the local EKF.
Asked by hank880907 on 2023-07-06 08:06:35 UTC
Updating to 3.5.1-2 20230*623* from 3.5.1-2 20230*525* fixed this issue. I'm not sure why, but am glad it is now working.
Asked by brow1633 on 2023-07-06 09:51:27 UTC
Comments
x-posted: https://robotics.stackexchange.com/questions/24974/robot-localization-gps-causing-large-drift-covariance-spike
Asked by brow1633 on 2023-07-06 07:04:46 UTC