Vision coordinate error strange formula

I am providing vision coordinates using the mavlink_msg_global_vision_position_estimate_encode and it works well up to 200-300m. I also provide the xy covariance as the square of the position error. I provide Z covariance as big value. I noticed that if I set Z covariance from big values to 1m^2 the drone flight changes, it flies more precisely even if I don’t use Z vision source (I use baro height). I searched the sources and found the strange (up to me) formula:

posErr = cbrtf(sq(covariance[0])+sq(covariance[6])+sq(covariance[11]));

in GCS_MAVLINK::handle_common_vision_position_estimate_data

I see that this is the only result that calculated from the covariance matrix for position error.

I see that position error (for XY as well?) was calculated using Z error! So, if I set Z error to a lower value the XY error becomes much lower! And the strange units used, if the covariance is m^2, then pos error units are is m^(4/3).
The strange is that if I set a big value to Z error, the posError clamped to 100, but the drone still flies well (but differently from Z error = 1…2 m). I just want to understand the logic behind that.

I tried to use the odometry messages, but it never worked, no reaction to my odometry messages.

The problem I need to solve is the stable flight above 500m. I have a circle-like motion there, so I want to understand all that deeper.

I already solved it using GNGP (works up to 1km, but the drone moves slowly randomly in 5% of the height circle), but I want to solve it with the more native method (for visual navigation). Navigation and RTL already work but for <400m of height. I need arbitrary height.

2 Likes

Hi @AndrewShpagin,

I’ve recently created this developer focused MAVLink interface page for Non-GPS navigation which might help a bit although it looks like it incorrect says the covariance fields are not used (I’ll fix that)

The ODOMETRY message is the preferred message but the vision-position-estimate and vision-speed-estimate messages should work just as well.

Yes, you’re right that we don’t separate the XY position error from the Z position error. Even deep down in the EKF3 where we do the XY fusion and Z fusion we can see they both just use the same “posErr” value that’s been passed in.

The source of the logic comes from a discussion with @priseborough and surely we could have separate XY from Z but perhaps we thought that it wasn’t necessary and wanted to keep it simple. We could change it if you’d like. A short term workaround would just be to provide the same position error for all 3 axis (perhaps an average of the 3) in the three critical elements fo the “covariance” grid (element 0, 6 and 11).

In a general sense if the position error is set too low then the resulting position estimate will be aggressively moved towards the sensor value and this can lead to a poor estimate of other values (like speed, acceleration). If the position error is set too high then the position estimate will tend to drift more than it should. You could even see an oscillation in the position estimate.

Just for reference MAVLink’s VISION_POSITION_ESTIMATE page says this about the covariance field (which is an array of 21 floats)

Row-major representation of pose 6x6 cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array.

Thanks a lot!

From your link I see that error clamped to 10, not 100, right? It may cause some problems on huge heights where errors may be really big. But it explains all - if I set big Z error, xy error is just 10, not 100, so it may work at least in simple cases, like on the ground.

But cube root in formula still not understood, even if it may be reversed and used as expected)

1 Like

Hi @AndrewShpagin,

I’ll have to defer to @priseborough as to why a cuberoot is used.

Re the 10 limitation we can definitely increase that if necessary. My guess is that it is somewhat arbitrary and was based on the types of external systems we had seen. With newer high-altitude-non-GPS systems their error can be much larger and still be useful.

Are you able to increase that limit yourself and see if it helps? I think we’d only want to increase the limit if it’s actually helpful/required.

1 Like

Thanks! We will test it and report it. Probably I will reverse the cube root formula to provide real distance error as

covariance_xyz = D^(3/2)/sqrt(3)

1 Like

Hi @AndrewShpagin,

I’ve created a wiki PR to improve the docs around the ODOMETRY message’s use of the covariance message and at some point soon-ish I hope to add a section specific to the vision-position-estimate message handling

Txs again for the reports

2 Likes

I made corrected SW, so waiting for test result from friend.
As for me it’s not good to use same errors and limits for all axises. In my opinion XY pos is one controll system, Z pos is other one.

2 Likes

We tested and that worked on 1000m, we changed 10m->50m max, of course error is table-based (dependent on height nonlinearly) + several other tricks to prevent circular motion, but that was good on 1000m. So, could be good to introduce the parameter for the upper limit.

1 Like

As we said before- IQ 450 much better as 300 :joy:
50 max - is it max error in AP or given by ExtNav table error?

1 Like

Ha, hi, my friend! ))) Glad to see you there!
50 is the max error I set instead of 10. As I know, you set 100, and this is good as well)
The actual passed error grows gradually with height (I made a table). Of course, I inverted the cube root formula. The result is just fantastic - less than 10m of real positioning precision on 1km with wind around 10-12m/s (17 degrees of drone tilt).

2 Likes

I’m not Programmer or IT, but I told you- as for me something strange in this section.
And if ExtNav use positionning errors- we need give them. It’s like HDOP/VDOP in GPS. Otherwice drone will “fly around” trying to reach exact unreacheble position.
Really glad that we fixed it!

Special thanks to AP Dev Team for they’r hard work!

2 Likes