Position Drifts after 60 Seconds when using External Vision

[jbdaniel18]

Hello,

I have been working to fuse external vision into the state estimator. For some reason at around 60 seconds+ X and Y begin rapidly diverging and then the EKF stops outputting a horizontal estimate. The drone is currently just sitting on my desk and the VO output is steady and consistent. Any suggestions on what to do to stop this behavior? I have checked all of the innovation test ratios and everything is small and consistent. Covariances are small for the visual odometry.

State Estimate

Visual Odometry Output

Velocity Plot

Justin Daniel

[julianoes]

FYI @kamilritz

@jbdaniel18 could you add a logfile from logs.px4.io?

[kamilritz]

@jbdaniel18 at which rate are you sending the odometry information?

[jbdaniel18]

I have included the log file. The VO outputs anywhere between 2 - 5 hz. However, it is always after 60 seconds that the EKF stops outputting a horizontal estimate.

https://review.px4.io/plot_app?log=e66acad8-21b5-44b2-ac62-0338d07ea979

[kamilritz]

It suddenly stops fusing the VIO data, you can see that the innovation & -variance stays suddenly constant. I see not anything that would explain this. But the sampling regularity of the sensor data looks not normal. @bresch could that sample irregularity points to something? @jbdaniel18 what fc are you using?

[jbdaniel18]

I am using a Pixracer R15

[jbdaniel18]

I tried to swap to a Pixhawk 4 and saw similar behavior.
https://review.px4.io/plot_app?log=28b856c1-00a7-4f06-b183-2c613ef79da5

[kamilritz]

@jbdaniel18 What kind of firmware version are you using? The one in the second log seems to be old. Make sure you use at least 1.10 or try master.

[jbdaniel18]

@kamilritz I did update the firmware on the Pixracer to 1.10 and I still saw the same results.

[kamilritz]

@jbdaniel18 Could you run it again with the master branch on the pixhawk 4? Notice that with 1.10 a lot of changes to the VIO related parts in the software happened. Before 1.10 there was no vision velocity fusion available. Until now I was never able to see any visual odometry velocity information in the log - no idea why this is - but could you may be check it through the mavlink console on the running vehicle by calling listener vehicle_visual_odometry.

[jbdaniel18]

@kamilritz Alright I will try to update the Pixhawk 4. Is the master branch different than 1.10?

[kamilritz]

It is very similar in terms of functionality, at least it should be. But since 1.10 a few things in the logging of the VIO data changed and it is easier for me to investigate the log.

[jbdaniel18]

@kamilritz https://review.px4.io/plot_app?log=033f7fe4-2eb7-40ed-84b7-c4c02735200a

Updated the Pixhawk 4. This time it did seem to track the VO for longer before diverging. However I still do not see the visual odometry velocity when I call listener vehicle_visual_odometry. I am using mavros and publishing the vision topic on /mavros/vision_speed/speed_twist_cov

[kamilritz]

Oh, that is probably it. I am not really sure, but I think if you send both vision position and vision speed separate, this will cause issues. You should use the mavros odometry pluging. With this you can send both position and velocity together.

[jbdaniel18]

I swapped the topic to /mavros/odometry/out and I can now see the VO velocity. However I still have the same behavior with the EKF.

https://review.px4.io/plot_app?log=3d34f071-84d2-464d-90fd-fefb07ef6a9e

[kamilritz]

Is this again on the pixracer? It does also not seem to be a new version of the firmware.

[jbdaniel18]

No it is on the Pixhawk4. It should be 1.10.1

[kamilritz]

Ok, I just thought as the sensor sampling was again very irregular. Could by any chance use current master or the v.1.11 candidate? This would help me to investigate.

[kamilritz]

Something else you can check is if the same problem persists if you disable the vision velocity fusion.

[wagnera]

@kamilritz I am working with @jbdaniel18 on this. I tried both the V.1.11 beta (1.11.0rc) and current master and on both the x and y position estimates remain at 0. listener vehicle_visual_odometry returns never published. And I see some Estimator Source 0 not supported MAVLINK messages.

[wagnera]

I was able to fix the issue of it not working with 1.11. Issue was with Mavros not outputting the correct estimator type. Original problem still exists. See attached log.

https://review.px4.io/plot_app?log=3368238f-0d1b-4a1f-b623-df1c9d1cbdb9

[kamilritz]

This is really odd. There is no sign that would indicate why it stopped fusing any VIO data. My only guess: The VIO data is provided at a rather low rate (4-5 HZ). I never used the estimator with VIO data at a such a low rate. Do you have the possibility to increase the rate to around 30 Hz?

[wagnera]

We switched to publishing at 30hz and we still see the same issue. Sampling irregularity looks much better. We also see an error at 57sec about the EKF ev hgt timeout. Log: https://review.px4.io/plot_app?log=8e71f81a-b465-4ddd-911a-3c0d8b6b80e0

Better irregularity:

We are just publishing our 5hz data at 30hz (just sending previous value until new one arrvies). But we notice the data around 52 seconds switches:

[jbdaniel18]

@kamilritz interesting that the VO update rate changes in the EKF. The VO is publishing at a constant 30Hz now but it almost looks like the EKF starts downsampling the data right around 52 seconds. Also the same point the horizontal estimate begins to diverge.

[kamilritz]

@jbdaniel18 It is not the EKF that is downsampling the data. The vehicle_visual_odometry gets logged before it enters the EKF. But this is really odd and not my field of expertise. Maybe @TSC21 or @mhkabir can help here. Just to be sure: You are sending now position and velocity together with the mavros odometry plugin, right?

[jbdaniel18]

@kamilritz I understand. Thanks for clearing that up. Yes we are publishing the data on the topic /mavros/odometry/out (nav_msgs::Odometry) so the velocity and position are being sent together to the mavros odometry plugin.

[mhkabir]

It looks like something is happening to your data which is local to your setup (you can see it in the raw vehicle_visual_odometry). You'll need to debug that first.

[jbdaniel18]

@mhkabir I am not sure I see the data issue. are you referring to the flat peaks on the first half of the graph? If that is what you are referring to it is because I am just republishing the last value received from the VO to up the rate from 5hz to 30 hz to see if the slow data rate was the source of the issue

[jbdaniel18]

What is strange to me is it looks like the horizontal estimate starts diverging right as the sampling rate of the VO data changes. I have verified the VO is producing data at 30 HZ, however right around 27 seconds the position begins to diverge and you can see a change in the data frequency as it is being logged as vehicle_visual_odometry

[mhkabir]

I'm referring to the change that occurs between the first and second half of the graphs. That is seemingly completely external to PX4 and happens in the data you're sending over MAVLink.

Or logging just silently dies somehow. I don't see dropouts reported.

[jbdaniel18]

I am tracking. That is the part I am stuck on as well. I am publishing the data to the MAVROS topic /mavros/odometry/out at a rate of 30 hz and can see from the ros bag there is no change in the msg frequency. Is the data being logged as vehicle_visual_odometry the raw data that is sent over MAVLink? QGC also shows the data being received at 30Hz the entire time.

[mhkabir]

I suspect something related to timesync. Can you try disabling NTP time on the companion side if it's enabled? e.g on Ubuntu 18.04 sudo timedatectl set-ntp no and restart mavros and the autopilot?

[wagnera]

The time sync was close. I had forgotten to update the time stamp header in messages so while being published at 30hz the time stamps were from the original message. Updating the time stamp at the time being published seems to have fixed the issue. It has sat for a few minutes with no issue.

Since this means the original problem was the low publishing rate of the vision odometry I would recommend that the documentation states the minimum rate required so other don't run into this problem.

[jbdaniel18]

I am curious, what is the minimum update rate that is required for external vision?

[kamilritz]

Since this means the original problem was the low publishing rate of the vision odometry I would recommend that the documentation states the minimum rate required so other don't run into this problem.

Actually it does: https://dev.px4.io/v1.9.0/en/ros/external_position_estimation.html#px4-mavlink-integration

[kamilritz]

I am curious, what is the minimum update rate that is required for external vision?

Would you help me to find out. Just try always lower rates until it diverges.

[stale[bot]]

This issue has been automatically marked as stale because it has not had recent activity. Thank you for your contributions.

[junwoo091400]

Just came across this issue, did you have any findings @kamilritz? I've had similar experiences when I set the axis wrong on VO data (especially through conversions, I messed up the XYZ components), but I've also experienced this with a correct setup sometimes. So I am suspecting an issue in EKF2, but no idea why for now :/

[Jaeyoung-Lim]

@junwoo091400 Please post a log of the issues you are having.

[junwoo091400]

This experience was from a while back, and I unfortunately don't have the logs from the past 😕 @Jaeyoung-Lim

[bresch]

That issue is quite old now, let's close it and re-open a new one if needed

[HaoChen1016]

@jbdaniel18 Hey, I faced the almost same issue with you and could I know that have you solved your issue?

[mengchaoheng]

@jbdaniel18 @kamilritz @mhkabir @wagnera @junwoo091400 @Jaeyoung-Lim can you help me please! https://github.com/PX4/PX4-Autopilot/issues/21504