Issues with odometry in robot_localization

asked 2018-11-30 10:07:25 -0500

marinePhD
40 ●7 ●8 ●13

updated 2019-01-10 11:29:19 -0500

gvdhoorn
86574 ●283 ●1432 ●1054 http://cor.tudelft.nl/

I'm using the robot_localization package with ROS Kinetic and I'm trying to fuse the IMU and GPS data from the Matrice M100 similar to the work done here.

My issue is the odometry/gps topic looks wacky and isn't in line with what I expect the output to be. If you take a look at the picture below, red is the real drone odometry, purple is the odometry/filtered_map topic and green is odometry/gps. I can't upload files yet but you can see it at this link. Occasionally, I experience jumps but nothing too big at all. I recorded a bag file and plotted the coordinates of the drone and raw GPS and gps/filtered seem to be close enough as seen here: however, the 3d position messages on the odometry topic is inaccurate as shown here: I'm not sure if I'm doing something wrong in the configuration .

I've also attached a copy of the bag file here: Any help is appreciated.

Here's a sample of my dual_ekf_navsat file:

ekf_se_odom:
  frequency: 30
  sensor_timeout: 0.1
  two_d_mode: false
  transform_time_offset: 0.0
  transform_timeout: 0.0
  print_diagnostics: true
  debug: false

  map_frame: map
  odom_frame: odom
  base_link_frame: m100/base_link
  world_frame: odom

  odom0: dji_odom
  odom0_config: [true, true, true,
                 true, true, true,
                 true,  true,  true,
                 true, true, true,
                 true, true, true]
  odom0_queue_size: 10
  odom0_nodelay: true
  odom0_differential: false
  odom0_relative: false

  imu0: imu/data
  imu0_config: [false, false, false,
                true,  true,  true,
                false, false, false,
                true,  true,  true,
                true,  true,  true]
  imu0_nodelay: false
  imu0_differential: false
  imu0_relative: false
  imu0_queue_size: 10
  imu0_remove_gravitational_acceleration: true

  use_control: false

  process_noise_covariance: [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.06, 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.06, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0.025, 0,     0,    0,    0,    0,    0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,     0.025, 0,    0,    0,    0,    0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,     0,     0.04, 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.02, 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 ...

(more)

answered 2019-01-10 08:49:49 -0500

Tom Moore

flag of United Kingdom of Great Britain and Northern Ireland

13689 ●74 ●240 ●232 http://github.com/ayrton04

updated 2019-01-14 03:59:19 -0500

Please post sample messages from every sensor input.

Can you explain what is wacky in the images you posted? It looks to me that the state estimate is closely tracking your odometry, and that the GPS odom values are just bound to the ground plane. You have zero_altitude set to true, so that makes sense. And remember that there are no headings associated with the GPS poses, so they won't look the same.

Anyway, another issue with your configuration is that you are fusing absolute X, Y, and Z position from your odometry, as well as absolute X, Y, and Z position from your GPS data. If your odometry is subject to drift (and I assume it is), then you will reach a point where those positions don't match. When that happens, the filter will start leaping back and forth between the two measurement sources. An alternative symptom - and I think you're seeing it - is that if your odometry covariance is much smaller than your GPS position covariance, the filter is going to effectively follow the odometry values. But again, I can't really say that with any certainty until you post sample messages.

Also, it looks like you left the navsat_transform_node yaw_offset comment in there from the sample config file, but changed the yaw_offset value to 0. Does your IMU read 0 facing east?

Finally, if your odometry source is internally integrating velocity data to get position, and then you fuse both the position and velocities in the EKF, you're biasing the EKF. If I were you, I'd set the first three values (X, Y, and Z) to false in your EKF configuration for dji_odom.

EDIT in response to updates:

OK, a few things:

Your dji_gps data is given in the gps_link frame. I assume a transform exists from m100/base_link to gps?
You imu_data topic is in the body_FLU frame. I assume you have a transform from m100/base_link to body_FLU?
Your dji_odom pose data is given in the map frame, yet you are trying to fuse it absolutely (i.e., the pose data, and not the velocity data) into your odom frame EKF. That's going to require the map->odom transform, which the map frame EKF is itself producing. Since you have an IMU providing your orientation, I would recommend that you fuse only velocity data from dji_odom in both your EKF instances. That, or (a) change the frame_id to odom in your dji_odom topic, (b) fuse only the pose data from dji_odom, and (c) fuse only velocity/acceleration data from the IMU (a, b, and c are for the odom frame EKF; for the map frame, I'd flip them and use velocity from the odometry and orientation from the IMU).
The w component of a quaternion isn't the same as the yaw component of orientation, so you'll need to convert that quaternion to Euler angles to get the offset. My guess is that you just want pi ...