robot_localization with GPS causes big drifts
Hi, I am attempting localization with GPS. Currently, my odom->baselink is working well and accurate. but map->baselink causes big jumps in rotations whenever GPS messages come in and it worsens during rotations. My IMU returns 0 when facing east. Magnetic declination radius is also close to 0 in my testing area
https://drive.google.com/file/d/1I7eXMGJPW7ENnJZDwOpGQSlH6Xj_rLDp/view?usp=share_link
UPDATE 1: reducing the global ekf's process noise covariance seems to reduce the speed in which the orientation changes to accomodate the direction of travel according to the GPS but still seem to have some misalignment in the overall orientation
ekf_filter_node_odom:
ros__parameters:
use_sim_time: true
frequency: 30.0
sensor_timeout: 0.1
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: false
debug: false
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom
odom0: /lidar_odometry
odom0_config: [true, true, true,
true, true, true,
false, false, false,
false, false, false,
false, false, false]
odom0_queue_size: 1
odom0_nodelay: false
odom0_differential: false
odom0_relative: false
twist0: /gyro_twist_with_covariance
twist0_config: [false, false, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]
twist0_queue_size: 1
twist0_nodelay: false
twist0_differential: false
twist0_relative: false
use_control: false
dynamic_process_noise_covariance: true
smooth_lagged_data: true
process_noise_covariance: 1.0 for diagonal
initial_estimate_covariance: 1.0 for diagonal
ekf_filter_node_map:
ros__parameters:
use_sim_time: true
frequency: 30.0
sensor_timeout: 0.1
two_d_mode: true
transform_time_offset: 0.15
transform_timeout: 0.0
print_diagnostics: false
debug: false
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: map
odom0: /lidar_odometry
odom0_config: [true, true, true,
true, true, true,
false, false, false,
false, false, false,
false, false, false]
odom0_queue_size: 1
odom0_nodelay: false
odom0_differential: true
odom0_relative: false
twist0: /gyro_twist_with_covariance
twist0_config: [false, false, false,
false, false, false,
true, true, false,
false, false, true,
false, false, false]
twist0_queue_size: 1
twist0_nodelay: false
twist0_differential: false
twist0_relative: false
odom1: /odometry/gps
odom1_config: [true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]
odom1_queue_size: 1
odom1_nodelay: false
odom1_differential: false
odom1_relative: false
use_control: false
dynamic_process_noise_covariance: true
smooth_lagged_data: true
process_noise_covariance: [x=1.0 | y=1.0 | z=1e-3 | r=0.3 | p=0.3 | yaw=0.01 | vx=0.5 | vy=0.5 | vz=0.1 | vr=0.3 | vp=0.3 | vyaw=0.3 | ax=0.3 | ay=0.3 | az=0.3]
initial_estimate_covariance: default
navsat_transform:
ros__parameters:
use_sim_time: true
frequency: 10.0
delay: 1.0
magnetic_declination_radians: 0.0
yaw_offset: 0.0
zero_altitude: false
broadcast_cartesian_transform: true
broadcast_utm_transform_as_parent_frame: true
publish_filtered_gps: true
use_odometry_yaw: false
wait_for_datum: false
wheel odometry + imu:
header:
stamp:
sec: 1669631155
nanosec: 43059495
frame_id: base_link
twist:
twist:
linear:
x: 1.160753966440119
y: 0.0
z: 0.0
angular:
x: -0.01817941665649414
y: 0.03200844302773476
z: -0.14795809984207153
covariance:
- 0.001
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 10000.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 10000.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0009
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0009
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0009
---
lidar odometry:
pose:
position:
x: 45.02977878252862
y: -10.513461437182793
z: -0.02517985660098549
orientation:
x: -0.0009699142460926872
y: 0.03354231215697803
z: -0.9191844680524953
w: -0.3923950640011253
covariance:
- 5.256737665051335e-06
- -4.156041364437555e-09
- -1.7666912868515172e-08
- -3.65483290419155e-08
- -1.8252792150939747e-08
- -3.3666444308625707e-10
- -4.156041393130758e-09
- 5.234027732804007e-06
- -2.734218919154185e-07
- 1.9404542678039115e-08
- -2.5603913476208935e-08
- 5.997112901078748e-09
- -1.766691286974723e-08
- -2.734218917906183e-07
- 9.607752901500373e-06
- 5.336091269701327e-09
- 1.021089431260967e-08
- 1.0837599543299836e-09
- -3.6548328998589445e-08
- 1.9404542020299608e-08
- 5.336087929861983e-09
- 1.0552562594174467e-05
- 9.052627116982032e-08
- 3.674071577117643e-09
- -1.8252792107848005e-08
- -2.560391320683213e-08
- 1.0210891865465687e-08
- 9.052627117985452e-08
- 1.0879033110682987e-05
- 3.3748829291199057e-08
- -3.3666445159551035e-10
- 5.997112897311334e-09
- 1.083760034924757e-09
- 3.674071579148742e-09
- 3.374882928961719e-08
- 1.0656027926163483e-05
---
odometry/gps
header:
stamp:
sec: 1669631153
nanosec: 359099642
frame_id: map
child_frame_id: ''
pose:
pose:
position:
x: 49.47389939068929
y: -9.435762428502429
z: -1.3579132356648063
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
covariance:
- 0.050176000000000005
- -3.868892054738017e-19
- 0.0
- 0.0
- 0.0
- 0.0
- 1.3954644856508374e-18
- 0.050176
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.052441
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
twist:
twist:
linear:
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0
covariance:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
---
gps fix:
header:
stamp:
sec: 1669631153
nanosec: 359099642
frame_id: gps_link
status:
status: 0
service: 1
latitude: 1.2812036
longitude: 103.86521859999999
altitude: 15.315
position_covariance:
- 0.050176000000000005
- 0.0
- 0.0
- 0.0
- 0.050176000000000005
- 0.0
- 0.0
- 0.0
- 0.052441
position_covariance_type: 2
---
Asked by MrOCW on 2022-11-28 12:37:39 UTC
Answers
I am using robot_pose_ekf but am going to use a similar implementation. Does your GPS have an internal kalman filter? What NMEA message is it using? Are you using RTK? Not using these will result in large jumps in fix data. For the robot_pose_ekf, you need to set a very high covariance for some GPS data so it isn't used in the EKF, and it appears you did that as well.
Asked by chased11 on 2022-11-29 18:10:48 UTC
Comments
using https://github.com/KumarRobotics/ublox ZEDF9-R
Yes there is RTK
There is no internal kalman filter in use
Asked by MrOCW on 2022-11-30 00:02:01 UTC
Comments