EKF not converging towards external vision source

GLange · May 10, 2023, 1:10pm

Hi,

i’m trying to get my quad set up with an external vision system for indoor flying.
Vision position data looks reliable and is provided via:

where ‘tvecs’ holds the current position (FRD and NED are aligned) in mm.

At the same time i’m reading the EKF position estimate via:

Moving the drone up/down, front/back, left/right yields the plots below where blue shows the vision estimate and orange the EKF estimate. While the EKF tracks the up/down, the other directions don’t seem to care for vision data.

EKF2_AID_MASK

EKF2_EVP settings assuming this forces the EKF to consider the EV data to be noise"free" and valid.

Any suggestions how to fix this issue?

Artem_Borisov · May 10, 2023, 4:07pm

Hi!
When I use EV:
EKF2_AID_MASK = 24
GPS disable
EKF2_HGT_MODE = Vision.
and that’s enough for me.

I have a question for you, how did you fill in the matrix-covariance? I did not understand this and set the value to NaN.

GLange · May 10, 2023, 4:37pm

Diagonal holds the variances of pose (position and angle) measurement. The upper triangle are the covariances of the measurements (i’m assuming them zero). Lower triangle doesn’t matter since it’s symetric.
MAVSDK takes this as an 1D-array starting top left and going row-wise through all 21 relevant entries.
I’m setting the position variance to 0.01 since that’s the smallest allowed value and i want the EKF to trust my position measurement. Angle variance is set to 100 since i’m not measuring it and i don’t want the EKF to trust the dummy zeros i feed it.

JulianOes · May 11, 2023, 8:38pm

I’m afraid I don’t know how this all works. The only pointer I can give you is how the MAVSDK input is assembled into the MAVLink message:

github.com

mavlink/MAVSDK/blob/e4e9c71ddcdd3e066810edf2292c941756c5ee91/src/mavsdk/plugins/mocap/mocap_impl.cpp#L78-L91


      
          // The covariance matrix needs to have length 21 or 1 with the one entry set to NaN.
          
          
if (vision_position_estimate.pose_covariance.covariance_matrix.size() == 21) {
              std::copy(
                  vision_position_estimate.pose_covariance.covariance_matrix.begin(),
                  vision_position_estimate.pose_covariance.covariance_matrix.end(),
                  covariance.begin());
          } else if (
              vision_position_estimate.pose_covariance.covariance_matrix.size() == 1 &&
              std::isnan(vision_position_estimate.pose_covariance.covariance_matrix[0])) {
              covariance[0] = NAN;
          } else {
              return Mocap::Result::InvalidRequestData;
          }

GLange · May 12, 2023, 10:20am

Disabling GPS seems to solve the issue.
However I don’t understand why it has an effect at all in an indoor situation without GPS lock.
Drone is ment to be used with the MoCap system in combination with a GPS at a later stage. So while giving some insight now, disabling won’t be a fix.

JulianOes · May 14, 2023, 9:34pm

Ok, I don’t know. I can ping @bresch, he might know.

Benja · May 15, 2023, 4:22pm

As far as I know, GPS and EV fusion should is possible starting form v1.14-beta1, not before. I never tried it personally though. Here are the references for the new documentation:
GPS: Using the ECL EKF | PX4 User Guide
EV: Using the ECL EKF | PX4 User Guide

And you must ensure that the NED frame of your EV is truly aligned to the true North.

bresch · July 5, 2023, 3:29pm

Precision on this: it also works if not aligned to North but you need to specify the frame in the message (e.g.: local FRD) and specify the current attitude. That way the EKF will rotate the measurements in NED before fusing it.