Hello everyone,
I am currently using EKF2 with external vision odometry data from a Qualisys motion capture system as the main pose source for my underwater vehicle (BlueROV2 Heavy).
The Qualisys system provides position and orientation data, which I first feed into a ROS 2 network. A simple EKF on the ROS 2 side estimates linear and angular velocities, and all four components (position, orientation, linear and angular velocity) are written into a px4_msgs/msg/VehicleOdometry message. This message is then passed to PX4 via the Micro XRCE-DDS Agent. The EKF2 output odometry from PX4 is used as the main control reference for my system.
The issue I am facing is that the EKF2 odometry drifts heavily and eventually becomes completely inconsistent with the ground truth from Qualisys.
I conducted three experiments, each logged and recorded. For all three cases, I have ULog files, Video recordings of my underwater vehicle, and RViz recordings showing the Qualisys ground truth (thin, longer frame) and the EKF2 odometry output. Note: RViz visualizes at low frequencies, the true odometry is published with 50 Hz.
(1) Steady state (after startup):
The EKF2 position estimate appears roughly correct, but the orientation shows a small offset compared to the Qualisys ground truth.
(2) Manual motion with drift:
When I move the vehicle by hand, EKF2 initially tracks the motion capture data fairly well, but soon starts to drift away, then occasionally recovers, and drifts again after a few seconds.
(3) After previous experiments / reboot:
Following the previous runs, EKF2 no longer aligns with the ground truth at all. The estimate is completely off and remains wrong even after rebooting PX4 or the entire vehicle. You can see in the log file that EKF2 appears to assume the vehicle’s steady state position is at the origin, which makes no sense because the inertial frame origin is located in the corner of the water tank.
I would be very grateful for any help or insights.
-
What could cause the initial orientation offset in (1)?
-
Why does EKF2 drift and partially recover in (2)?
-
And what could explain why EKF2 completely loses trust in external vision data in (3), even after a reboot?
My goal is to achieve a robust EKF2 fusion of external vision data that remains stable and consistent with the Qualisys ground truth.
Thank you!