@dwiel just to be sure, can you also plot the T265 position data (VISO)? From my experience, the T265 data usually correlates with the observations local_position_ned data, given that there are no other positioning data.
At the moment there are developments on multiple fronts:
- It is now possible to use vision data with EKF3 and reported results seem ok so far. The EKF3 can also handle the position reset when it happens (the glitch).
- The position + attitude data (
VISION_POSITION_ESTIMATE
message) is just one possible type of data. With EKF3 you can also useVISION_POSITION_DELTA
(changes in position and orientation, now supported by the T265 script and this cpp implementation) and maybeVISION_SPEED_ESTIMATE
orODOMETRY
might be available soon.
If you can have a try, maybe one of these will give you better results.