Revision history [back]

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

Please post sample messages from every sensor input.

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'r~~ 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/2, and then your magnetic declination correction value will take care of the rest.

Overall, my general advice with these applications is to start small. Start with a single EKF instance, and just the odom data. Convince yourself that the data is being fused properly, then add in the IMU. Check if the output changes as you'd expect from the odometry-only run. Make sure the run is long enough to encompass a lot of turns, such that your odometry yaw will differ from the IMU yaw, thereby ensuring that you aren't fusing absolute yaw from two sources.

When you're convinced everything is working, move to the map frame EKF. Repeat the process again: start with just odom, then IMU, then add the GPS last. When you add the GPS data, at first, _don't_ fuse it with the EKF. Just run navsat_transform_node and plot the output alongside your map frame EKF output. They ought to align for a while, until drift carries the EKF instance away. Then you can fuse the GPS data.