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.
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.
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.
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.