0
$\begingroup$

Rosanswers logo

Hi!

We've been trying to use the Robot Localization package, and we've been having trouble with the LIDAR data rotating in our map. Here is a video of the observed behavior - https://drive.google.com/file/d/1pKpCoeCftGF-mWE9WZPxVLQHszqQVqM4/view?usp=sharing

As for our system configuration, we have wheel encoders from which we compute Odometry. We have a Swift Nav Piksi from which we get a RTK GPS fix based position (x,y,z, no orientation). After some initial tests, we realized we were missing a heading input, so we took the raw IMU and Mag data from the Swift Nav Piksi, fed it into the imu_madgwick_filter node, to get an absolute heading value.

The jitters in the video is coming from the IMU, as the robot moves however it seems like the LIDAR data has a tendency to yaw clockwise. Removing the IMU as an input source removes the jitter but the LIDAR still yaws just as much.

We've two EKF's -

  1. A map EKF that fuses wheel odometry + IMU + GPS Fixed position and provides map -> odom transform and
  2. An odom EKF that fuses wheel odometry + IMU and provides odom -> base_link transform.

These are our config files -

ekf_map.yaml -

  frequency: 50               # frequency, in Hz, at which the filter will output a position estimate
  sensor_timeout: 0.1         # period, in seconds, after which we consider a sensor to have timed out
  two_d_mode: true            # no 3D information will be used in your state estimate
  transform_time_offset: 0.1 # provide an offset to the transform generated by ekf_localization_node.

  print_diagnostics: true     # If you're having trouble, try setting this to true, and then echo
                              # the /diagnostics_agg topic
  publish_tf: true            # Whether to broadcast the transformation over the /tf topic
  publish_acceleration: false # Whether to publish the acceleration state

  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: map           # Defaults to the value of odom_frame if unspecified

  # Fuse x and y velocities from wheel encoders
  odom0: /wheel_encoder/odom
  odom0_differential: false
  odom0_relative: false
  odom0_queue_size: 10
  odom0_config: [false, false, false,  # x, y, z
                 false, false, true,  # roll, pitch, yaw
                 true,  true,  false,  # x vel, y vel, z vel,
                 false, false, false,   # roll vel, pitch vel, yaw vel
                 false, false, false]  # x acc, y acc, z acc

  # Fuse Piksi INS orientation data
  imu0: /filtered_imu
  imu0_differential: false
  imu0_relative: false
  imu0_remove_gravitational_acceleration: false
  imu0_queue_size: 10
  imu0_config: [false, false, false,  # x, y, z
                false, false, true,  # roll, pitch, yaw
                false, false, false,  # x vel, y vel, z vel,
                false , false , false ,  # roll vel, pitch vel, yaw vel
                false , false , false ]  # x acc, y acc, z acc

  pose0: /piksi_rover/enu_pose_fix
  pose0_config: [true,  true,  false,
                 false, false, false,
                 false, false, false,
                 false, false, false,
                 false, false, false]
  pose0_differential: false
  pose0_relative: false
  pose0_queue_size: 10
  pose0_rejection_threshold: 5  # Note the difference in parameter name

  process_noise_covariance: [1e-2, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,       # x
                             0,    1e-2, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,       # y
                             0,    0,    1e-6, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,       # z
                             0,    0,    0,    1e-6, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,       # roll
                             0,    0,    0,    0,    1e-6, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,       # pitch
                             0,    0,    0,    0,    0,    0.01,  0,     0,     0,    0,    0,    0,    0,    0,    0,       # yaw
                             0,    0,    0,    0,    0,    0,    1e-4,  0,     0,    0,    0,    0,    0,    0,    0,       # x vel
                             0,    0,    0,    0,    0,    0,    0,     1e-4,  0,    0,    0,    0,    0,    0,    0,       # y vel
                             0,    0,    0,    0,    0,    0,    0,     0,     1e-6, 0,    0,    0,    0,    0,    0,       # z vel
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    1e-6, 0,    0,    0,    0,    0,       # roll vel
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    1e-6, 0,    0,    0,    0,       # pitch vel
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.02, 0,    0,    0,       # yaw vel
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.01, 0,    0,       # x acc
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.01, 0,       # y acc
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]   # z acc

  initial_estimate_covariance: [0.01, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,       # x
                                0,    0.01, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,       # y
                                0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,       # z
                                0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,       # roll
                                0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,       # pitch
                                0,    0,    0,    0,    0,    1.0,  0,    0,    0,    0,     0,     0,     0,    0,    0,       # yaw
                                0,    0,    0,    0,    0,    0,    1e-2, 0,    0,    0,     0,     0,     0,    0,    0,       # x vel
                                0,    0,    0,    0,    0,    0,    0,    1e-2, 0,    0,     0,     0,     0,    0,    0,       # y vel
                                0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,       # z vel
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,       # roll vel
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,       # pitch vel
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,       # yaw vel
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,       # x acc
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,       # y acc
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]    # z acc

ekf_odom.yaml -

  frequency: 50               # frequency, in Hz, at which the filter will output a position estimate
  sensor_timeout: 0.1         # period, in seconds, after which we consider a sensor to have timed out
  two_d_mode: true            # no 3D information will be used in your state estimate
  transform_time_offset: 0.0  # provide an offset to the transform generated by ekf_localization_node.

  print_diagnostics: true     # If you're having trouble, try setting this to true, and then echo
                              # the /diagnostics_agg topic
  publish_tf: true            # Whether to broadcast the transformation over the /tf topic
  publish_acceleration: false # Whether to publish the acceleration state

  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

  odom0: /wheel_encoder/odom
  odom0_differential: false
  odom0_relative: false
  odom0_queue_size: 10
  odom0_config: [false, false, false,  # x, y, z
                 false, false, true,  # roll, pitch, yaw
                 true,  true,  false,  # x vel, y vel, z vel,
                 false, false, false,   # roll vel, pitch vel, yaw vel
                 false, false, false]  # x acc, y acc, z acc

  # Fuse Piksi INS orientation data
  imu0: /filtered_imu
  imu0_differential: false
  imu0_relative: false
  imu0_remove_gravitational_acceleration: false
  imu0_queue_size: 10
  imu0_config: [false, false, false,  # x, y, z
                false, false, true,  # roll, pitch, yaw
                false, false, false,  # x vel, y vel, z vel,
                false , false , false ,  # roll vel, pitch vel, yaw vel
                false , false , false ]  # x acc, y acc, z acc

  process_noise_covariance: [0.05, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,       # x
                             0,    0.05, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,       # y
                             0,    0,    0.06, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,       # z
                             0,    0,    0,    0.03, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,       # roll
                             0,    0,    0,    0,    0.03, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,       # pitch
                             0,    0,    0,    0,    0,    0.15, 0,     0,     0,    0,    0,    0,    0,    0,    0,       # yaw
                             0,    0,    0,    0,    0,    0,    1e-3,  0,     0,    0,    0,    0,    0,    0,    0,       # x vel
                             0,    0,    0,    0,    0,    0,    0,     1e-3,  0,    0,    0,    0,    0,    0,    0,       # y vel
                             0,    0,    0,    0,    0,    0,    0,     0,     0.04, 0,    0,    0,    0,    0,    0,       # z vel
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0.01, 0,    0,    0,    0,    0,       # roll vel
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.01, 0,    0,    0,    0,       # pitch vel
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.02, 0,    0,    0,       # yaw vel
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.01, 0,    0,       # x acc
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.01, 0,       # y acc
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]   # z acc

  initial_estimate_covariance: [1e-9, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,       # x
                                0,    1e-9, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,       # y
                                0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,       # z
                                0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,       # roll
                                0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,       # pitch
                                0,    0,    0,    0,    0,    1.5,   0,    0,    0,    0,     0,     0,     0,    0,    0,       # yaw
                                0,    0,    0,    0,    0,    0,    1e-1, 0,    0,    0,     0,     0,     0,    0,    0,       # x vel
                                0,    0,    0,    0,    0,    0,    0,    1e-1, 0,    0,     0,     0,     0,    0,    0,       # y vel
                                0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,       # z vel
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,       # roll vel
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,       # pitch vel
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,       # yaw vel
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,       # x acc
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,       # y acc
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]    # z acc

imu_madgwick_filter-

    world_frame: enu
    fixed_frame: piksi_imu_device
    remove_gravity_vector: true
    orientation_stddev: 0.5
    use_mag: true
    publish_debug_topics: true
    gain: 0.2
    mag_bias_x: -0.000041
    mag_bias_y: -0.0000285
    mag_bias_z: 0.000012

Also attaching sample messages from each our input sources - wheel_encoder/odom -

header: 
  seq: 475
  stamp: 
    secs: 1592594309
    nsecs: 703321274
  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.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1]
twist: 
  twist: 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1]

/filtered_imu -

header: 
  seq: 49
  stamp: 
    secs: 1592594299
    nsecs: 354165077
  frame_id: "piksi_imu"
orientation: 
  x: -0.0121989609543
  y: -0.0193524866581
  z: 0.470692586114
  w: 0.882000655322
orientation_covariance: [0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1]
angular_velocity: 
  x: -0.000665810591447
  y: 0.0108527126406
  z: 0.0056593900273
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration: 
  x: 0.0372570168401
  y: -0.0547122337325
  z: 0.765938373061
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

piksi_rover/enu_pose_fix

header: 
  seq: 245
  stamp: 
    secs: 1592594319
    nsecs: 616166114
  frame_id: "enu"
pose: 
  pose: 
    position: 
      x: 0.00782178712532
      y: -0.00477637573402
      z: 0.0208077050416
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
  covariance: [0.0049, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0049, 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, 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]

A couple of points to note -

  • The co-variances in the IMU and wheel_encoder odom message are empirical, no calculation but they values make sense from tests that we've performed and seen. The covariance from the Piksi is coming from the device. I've looked at several questions, tried reducing the co-variances in the messages so the filter trusts sensors more than it's own prediction.
  • I tried setting the use_control param but haven't had much luck with that.
  • I also tried dynamic_profess_noise_covariance param, to no avail.

Pretty lost at this point, and unsure what is causing the LIDAR data to yaw like this.

Any help would be greatly appreciated!

Originally posted by Sidd on ROS Answers with karma: 155 on 2020-06-22

Post score: 0

Improve this question
$\endgroup$

2 Answers 2

Reset to default
0
$\begingroup$

Rosanswers logo

I see a couple things:

  1. You are fusing absolute yaw data from two sources, wheel odometry and IMU. Those aren't going to agree over time. So the filter will fuse a measurement from the wheel encoder odometry, and the filter will jump towards that yaw value. Then it will fuse a measurement from the IMU, and it will jump towards that yaw value. So the state estimate will jump back and forth.
  2. If, after disabling the IMU, you are seeing a slow rotation in one direction, that says your wheel encoder odometry must be doing the same thing. Try disabling the IMU yaw value, and then plotting the raw wheel encoder yaw value vs. the filtered yaw value, and see what you get. I'm guessing the wheel encoder data is going to more or less match the filter.
  3. Just checking: when you "turn off" the IMU, are you just commenting it out? Because the parameter will remain unless you restart the ROS core. You'd need to give it a false topic name that doesn't exist.

My recommendations here would be to:

  1. Ignore the map frame EKF for a moment. Get the odom frame EKF behaving the way you want (visualize it in the odom frame), and then move on.
  2. Unless your solution requires an earth-referenced heading for state estimation, I'd just fuse yaw velocity from the IMU and the wheel encoders.
  3. Part of the reason for (2) is that your RTK poses may not agree with your IMU heading (and it definitely won't agree with your wheel encoder heading over time). So if your RTK says you moved from (0, 0) to (1, 1), then your yaw should be 45 degrees. But your IMU might read 20 degrees, which would give the robot the appearance of moving laterally.

Originally posted by Tom Moore with karma: 13689 on 2020-07-10

This answer was NOT ACCEPTED on the original site

Post score: 3

Improve this answer
$\endgroup$
0
$\begingroup$

Rosanswers logo

Thanks for the pointers @Tom Moore, your suggestions helped narrow down the problem.

I got rid of the map frame EKF altogether to simplify things and used a single KF. Fused the absolute yaw from the IMU and yaw velocity from the wheel encoders. (We need an earth referenced heading in our system). This improved performance but the drift was not completely eliminated.

The culprit was the disagreement between the RTK poses and IMU orientations (Point 3 in @Tom Moore's recommendations). With some tedious testing and adding a manual fudge factor to the IMU reported yaw we were able to get the data (LIDAR data and robot odometry) aligned together.

For our long term solution, we procured a better IMU and have had superior performance since.

Originally posted by Sidd with karma: 155 on 2020-08-17

This answer was ACCEPTED on the original site

Post score: 0

Improve this answer
$\endgroup$

Your Answer

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