LIDAR data rotates when using EKF from Robot Localization

edit

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/1pKpC...

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 ...