0
$\begingroup$

Rosanswers logo

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: false
odom0_differential: false
odom0_relative: false

# -------------------------------------
# GPS Velocity:
# -------------------------------------
# twist0: /gps_vel_body
# twist0_config: [false,  false,  false,
#               false, false, false,
#               true, true, false,
#               false, false, false,
#               false, false, false]
# twist0_queue_size: 10
# twist0_nodelay: true
# twist0_differential: false
# twist0_relative: false


use_control: false
stamped_control: false
control_timeout: 0.2
control_config: [true, false, false, false, false, true]
acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]

#                            x     y     z     r     p     y   x_dot  y_dot  z_dot  r_dot p_dot y_dot x_ddot y_ddot z_ddot
process_noise_covariance: [0.10, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0.10, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0.03, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0.03, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0.03, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0.50, 0,     0,     0,    0,    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.1, 0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0.03, 0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0.01, 0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.01, 0,    0,    0,    0,
                           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.01, 0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.01, 0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.15]

EDIT2: The alignment problem was a Magnetometer problem. I have, now, a dual f9p GPS working as moving base and reporting HP heading.

image description

as you can see de pose from RL is not aligned with the motion in the vertical part, but it is aligned on the horizontal part.

image description The same dataset but using dual-gps heading solved the problem (I tunne a little noise covariance matrix too)

I can confirm that velocity reported from GPS is not delta_POS/delta_t. It has it's own covariance matrix so I added to RL filter and it's working ok.

Now, there is two issues left. The first one is how to fuse GPS heading in RL. Right now I have a madgwick filter with Gyro and Accel data. It output an IMU message including uncorrected orientation, ie not earth aligned. So right now I'm running a complimentary filter between Gyro data and heading from GPS and put that value on the yaw angle of the IMU message that is used on RL. But, I'm wondering is there a better way to fuse GPS heading on RL? maybe a new imu message with just YAW orientation and just enable YAW parameter in the yaml config file? then on the main imu enable ax, ay, az and vyaw?

The second one is about velocity estimation. It's not smooth and have some "spikes":

image description

image description

I'm working on PID controller for surge speed and yaw rate and the estimate makes a little difficult to tune PID constants. I'm not using accelerometer data, I have found that RL behave a little worse using accel data. My EKF config file is now this one:

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

# Further input parameter examples
# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az.
imu0: 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
              false, false, 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  #

# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set
# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame.
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: false
odom0_differential: false
odom0_relative: false

# -------------------------------------
# GPS Velocity:
# -------------------------------------
twist0: /chori/gps_vel_body
twist0_config: [false,  false,  false,
               false, false, false,
               true, true, false,
               false, false, false,
               false, false, false]
twist0_queue_size: 10
twist0_nodelay: true
twist0_differential: false
twist0_relative: false

use_control: false
stamped_control: false
control_timeout: 0.2
control_config: [true, false, false, false, false, true]
acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
process_noise_covariance: [0.01, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0.01, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0.03, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0.03, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0.03, 0,    0,     0,     0,    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.05, 0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0.05, 0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0.03, 0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0.01, 0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.01, 0,    0,    0,    0,
                           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.1, 0,    0,
                           0,    0,    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.15]

Thank and best regards


Originally posted by elgarbe on ROS Answers with karma: 106 on 2021-08-12

Post score: 1

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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?


Originally posted by Tom Moore with karma: 13689 on 2021-09-27

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by elgarbe on 2021-09-27:
I edit original question to include the information you request. Thank a lot Tom

Comment by Tom Moore on 2021-10-04:
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?

Comment by Tom Moore on 2021-10-04:
And maybe show some sample output?

Comment by elgarbe on 2021-10-07:
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.

Comment by elgarbe on 2021-10-07:
My big concern is why IMU is not aligned to the ASV trajectory and how to improve it.

Comment by elgarbe on 2021-12-08:
Updated the question Tom, thank

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.