ROS2 Node Publishing GPS Data Drifts Out of Bounds After Several Seconds

ROS2 Node: GPS and IMU Data Publishing - Position Drift Issue After Initial Accuracy

I'm developing a ROS2 node that reads GPS and IMU data from a file and publishes it to various ROS2 topics (sensor_msgs::msg::NavSatFix, geometry_msgs::msg::PoseWithCovarianceStamped, and sensor_msgs::msg::Imu). The node performs as expected at startup, with accurate data and correct pose behavior. However, after a few seconds, the published data starts to drift out of bounds, and the system begins displaying incorrect positions.

Problem Description:

• Expected Behavior: The GPS and pose data should remain accurate and within the expected area throughout the runtime.
• Actual Behavior: Initially, the object/vehicle is correctly positioned and appears to turn around as expected. After a few seconds, the position data starts to drift, and the object/vehicle eventually moves outside the expected bounds.
• Environment: ROS2, using a combination of GPS and IMU data for navigation and localization.

Code Overview:

Below is a simplified version of my node. The full node reads from a file, converts latitude/longitude to UTM coordinates, and publishes GPS, pose, and IMU data to the respective topics.

CODE: https://codefile.io/f/d0yZrBEtTL

Troubleshooting Attempted:

• Time Synchronization: Ensured the time difference (time - last_time) and the ROS time are correctly calculated.
• Loop Delays: Checked the loop's processing time and reduced the sleep duration to avoid timing issues.
• Coordinate Conversion: Verified that the UTM conversion is accurate and that the local coordinates are calculated relative to a correct origin.
• Covariance Settings: Reviewed the covariance values to ensure they accurately represent sensor uncertainties.

Specific Questions:

1. Why does the system appear to function correctly at the beginning but starts drifting out of bounds after a few seconds?
2. Could there be a cumulative timing or precision error in my loop that's causing this drift? How can I mitigate it?
3. What are some best practices to ensure the GPS and pose data remain accurate over time when processing and publishing data in a loop like this?

Additional Context:

• The problem seems to be time-related, as the drift occurs gradually after the node has been running for a few seconds.
• I'm using standard ROS2 tools and libraries, with the GPS data being simulated from a pre-recorded dataset.

Any insights or suggestions on how to debug or resolve this issue would be greatly appreciated!

GPS gives accurate results at low frequency, while IMU gives results at high frequency and the amount of error increases over time, so I think the reason for the drift is due to the IMU.

Possible causes and solutions:

1. There may be a timestep error. Check the GPS and IMU timestamps and make sure they are in the same time range.

2. The IMU may not be properly calibrated. Review the IMU outputs and update your Bias values.

3. Try running gyro and acceleration data through the Mahony orientation filter and using Kalman in a way that is compatible with GPS. This will allow the accumulated errors to be tolerated by GPS.

Note: Gyroscope data may cause drift, reduce Gyroscope confidence to avoid this

In this section you can find sample codes for IMU filtering.