Fusing GPS velocity with robot_localization
Hi, I'm working on an anmanned surface vessel, currently I have an IMU publishing RAW data, then a magnetometer and imu_filter_madgwick to get orientation. Finally I have a RTK GPS (F9P from ArduSimple) and using kummar ublox driver I have /fix messages. Right now I',m fusing all of this with RL, one ekf and one navsat node. It seems to work right on a static test (I can't make test on water yet). I saw that F9P is reporting velocity and wondering if I will get better results fusing this data. If so, what is the best way of doing it.
EDIT: here is a repo with config and logfile. Regarding velocity message:
header:
seq: 184
stamp:
secs: 1631812890
nsecs: 469710
frame_id: "gps"
twist:
twist:
linear:
x: -0.686
y: -0.353
z: -0.008
angular:
x: 0.0
y: 0.0
z: 0.0
covariance: [0.003969, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.003969, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.003969, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.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]
---
It's in ENU frame, but I have a node to rotate it to body_frame. The localization launch file is localize-offline.launch It brings up everything. I've tested adding this message and the result seems to not improve. In the bag file and current configuration it seems that there is some miss-alignment or something else, because on some parts of the path the IMU (and RL) seems to not align with the trajectory of the ASV. BTW, the test is an Anmanned Surface Vessel, controlling in MANUAL mode on shallow water. There was some wind and water current in the test. The GPS is e F9P RTK from ardusimple.
EDIT2: ekf config file
frequency: 30
silent_tf_failure: false
sensor_timeout: 0.1
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
debug_out_file: /path/to/debug/file.txt
publish_tf: true
publish_acceleration: false
map_frame: map # Defaults to "map" if unspecified
odom_frame: odom # Defaults to "odom" if unspecified
base_link_frame: base_link # Defaults to "base_link" if unspecified
world_frame: odom # Defaults to the value of odom_frame if unspecified
# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az.
imu0: imu/data #imu/data
imu0_config: [false, false, false, # x, y, z,
false, false, true, # roll, pitch, yaw
false, false, false, # vx, vy, vz
false, false, true, # vroll, vpitch, vyaw
true, true, false] # ax, ay, az
imu0_nodelay: false
imu0_differential: false
imu0_relative: false
imu0_queue_size: 10
imu0_pose_rejection_threshold: 0.8 # Note the difference in parameter names
imu0_twist_rejection_threshold: 0.8 #
imu0_linear_acceleration_rejection_threshold: 0.8 #
imu0_remove_gravitational_acceleration: true
# -------------------------------------
# GPS Odometry (From Navsat transform):
# -------------------------------------
odom0: odometry/gps
odom0_config: [true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]
odom0_queue_size: 10
odom0_nodelay ...