Ask Your Question
0

Fusing GPS velocity with robot_localization

asked 2021-08-12 20:49:32 -0600

elgarbe gravatar image

updated 2021-10-04 20:41:26 -0600

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 ...
(more)
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
1

answered 2021-09-27 11:20:13 -0600

Tom Moore gravatar image

updated 2021-11-23 04:18:42 -0600

In what kind of message is the velocity data being reported? You need to make sure the message type is supported by your r_l state estimation node, and you also need to make sure the sensor frames are specified correctly. Maybe if you provided some sample messages?

EDIT in response to updates and comments:

My big concern is why IMU is not aligned to the ASV trajectory and how to improve it.

In what way is it not aligned? Are you saying that the USV's heading in rviz does not point in the direction of its motion? This suggests that you perhaps your magnetic declination or yaw offset is improperly configured. You should include your navsat_transform_node configuration in the question as well. But I took a peek at the repo, and I see this:

yaw_offset: 0.0

That means that your IMU is reporting 0 radians when facing East, which is not common for IMUs. Are you sure that's correct? Can you show me a screen cap of what you're seeing?

edit flag offensive delete link more

Comments

I edit original question to include the information you request. Thank a lot Tom

elgarbe gravatar image elgarbe  ( 2021-09-27 12:37:03 -0600 )edit

OK, so you are able to fuse that message type in (it's a supported message format), but it's not helping? Would you mind just posting the full EKF config here, so the question contains all the information required?

Tom Moore gravatar image Tom Moore  ( 2021-10-04 03:06:17 -0600 )edit

And maybe show some sample output?

Tom Moore gravatar image Tom Moore  ( 2021-10-04 03:06:33 -0600 )edit

what kind of sample would you like to see? I could try to run two R_L instance, one with GPS_VEL and one without it in order to compare.

elgarbe gravatar image elgarbe  ( 2021-10-07 09:38:52 -0600 )edit

My big concern is why IMU is not aligned to the ASV trajectory and how to improve it.

elgarbe gravatar image elgarbe  ( 2021-10-07 09:41:35 -0600 )edit
0

answered 2021-10-05 04:44:06 -0600

Per Edwardsson gravatar image

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.

When deadling with EKF's, I tend to think in terms of information sources. The benefit of these filters is that you can add any number of separate sources of information and get usable data out. The problem here is that I suspect that the GPS module only uses GPS to produce its data, meaning that even though it looks like two different sources, it is actually just one. If so, adding both of them to your EKF will be the same thing as adding just one. Don't bother with the velocity in the EKF.

edit flag offensive delete link more

Comments

Yes, I was thinking the same, but then I saw Heron project and they use GPS velocity: https://github.com/heron/heron/blob/k...https://github.com/heron/heron/blob/k...

So, I have some doubt about the benefit of adding this messages.

Thank

elgarbe gravatar image elgarbe  ( 2021-10-07 09:37:24 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2021-08-12 20:49:32 -0600

Seen: 80 times

Last updated: Nov 23