EKF2 not working with (only) External Vision inputs

arthurBricq commented 2 years ago

Describe the bug

Hello everyone, I am not able to use EKF2 with inputs from my external VIO system. It seems like EKF2 is always trying to make the position (x,y) of the state converge to the origin (0,0). And the z part of the estimation is also wrong. I use the 1.13.0 release of the code on a pixhawk.

I followed the instructions of the documentation (refering to these links: https://docs.px4.io/v1.12/en/ros/external_position_estimation.html, https://docs.px4.io/v1.12/en/advanced_config/tuning_the_ecl_ekf.html) but I always encountered behavior where the EKF2 position simply does not follow the VIO output.

To Reproduce

I use a minimal setup with: a pixhawk, connected to a companion computer which sends odometry messages.

the odometry messages are well received and I can see them properly with MAV_ODOM_LP = 1 on the mavlink inspector. The odometry itself is not faulty (it was intensively verified)
the EKF2 has the following configuration:

param set EKF2_AID_MASK 280 (ev_evl, ev_pos, ev_yaw)
# or 
param set EKF2_AID_MASK 328 (ev_vel, ev_pos, rotate external vision)
# use vision for heigth fusion (same problem happens if I use barometer)
param set EKF2_HGT_MODE 3
# turn off the mag. fusion 
param set EKF2_MAG_TYPE 5
# the delay 
param set EKF2_EV_DELAY 150

The delay was found by rotating the drone around one of its axis and comparing the topics sensor_combined.gyroscope and vehicle_visual_odometry.

Expected behavior

I would expect the EKF2 filter to closely match the output of my external vision system (which is very precise), since I fuse no other additional data. But it really does not.

Log Files and Screenshots

Here I attach a log where I did some basic motions with my setup (crosses performed in the air, basically step functions of the drone): https://logs.px4.io/plot_app?log=1a9e5106-c592-48e4-aa23-d93627b1357d

We can see in the logs that the Visual Odometry Position describes this motion. However, when we look at the topic *Local Position X or Y, we see that the estimates always drift back to the (0,0) position.

It seems however that the speed estimate is pretty good.

I have tried many things but I can't find where is my mistake. Thanks in advance for the potential help ! Best

ecmnet commented 2 years ago

You might check the EKF2 solution status. It seems as if EKF2 rejects vision position data.

arthurBricq commented 2 years ago

Thanks for the reply ! It seems like you are right, here is the estimator status log. The control_mode_flags especially specifies that vision is only used for the height fusion.

TOPIC: estimator_status
 estimator_status_s
    timestamp: 3087359655  (0.012060 seconds ago)
    timestamp_sample: 3087358488  (1167 us before timestamp)
    vibe: [0.0000, 0.0000, 0.0005]
    output_tracking_error: [0.0000, 0.0002, 0.0015]
    control_mode_flags: 67125249 (0b0000'0100'0000'0000'0100'0000'0000'0001)
    filter_fault_flags: 0 (0b0000'0000'0000'0000'0000'0000'0000'0000)
    pos_horiz_accuracy: 0.0765
    pos_vert_accuracy: 37.6003
    mag_test_ratio: 0.0005
    vel_test_ratio: nan
    pos_test_ratio: nan
    hgt_test_ratio: 0.0005
    tas_test_ratio: 0.0000
    hagl_test_ratio: 0.0000
    beta_test_ratio: 0.0000
    time_slip: 0.0000
    accel_device_id: 3932170 (Type: 0x3C, SPI:1 (0x00)) 
    gyro_device_id: 3932170 (Type: 0x3C, SPI:1 (0x00)) 
    baro_device_id: 3997730 (Type: 0x3D, SPI:4 (0x00)) 
    mag_device_id: 396825 (Type: 0x06, I2C:3 (0x0E)) 
    gps_check_fail_flags: 0 (0b0000'0000'0000'0000)
    innovation_check_flags: 0 (0b0000'0000'0000'0000)
    solution_status_flags: 228 (0b0000'0000'1110'0100)
    reset_count_vel_ne: 1
    reset_count_vel_d: 6
    reset_count_pos_ne: 1
    reset_count_pod_d: 6
    reset_count_quat: 0
    pre_flt_fail_innov_heading: False
    pre_flt_fail_innov_vel_horiz: False
    pre_flt_fail_innov_vel_vert: False
    pre_flt_fail_innov_height: False
    pre_flt_fail_mag_field_disturbed: False
    health_flags: 0 (0b0000'0000)
    timeout_flags: 0 (0b0000'0000)

However I do not understand why the estimator would reject the odometry, do you how I can investigate this problem ? Thanks in advance !

Edit: I have tried to increase the vision gates (EKF2_EVP_GATE, EKF2_EVA_GATE) to much bigger values like 10, but the same problems persits. When I look at the innovations of the filter, I also notice that there absolutely zero innovation provided by the vision.

dagar commented 2 years ago

You have the EV noise set to 0. Screenshot from 2022-07-01 09-07-32

Where did you get EKF2_EV_DELAY 365?

dagar commented 2 years ago

What's the VIO system?

arthurBricq commented 2 years ago

Thansk for your feedback !

This was just one attempt accross the many different logs that I have recorded. I was trying different values of the noise and of the delay, but i guess i must have uploaded one of the wrong logs, I am sorry. The noises were not set to zero however, but just to a procesion lower than the one accepted by QGC.

Here is a new log with more realistic values for these variables: https://logs.px4.io/plot_app?log=a0aff5c1-e883-4eb9-beab-030e9d402569. The same problems occurs, that is

the (x,y) position always come back to zero
the estimator_status say that ev_pos is not used
even the z estimate is not good

Here are the parameters for EKF_EV* used in the log right here !

nsh> param show ekf2_ev*
Symbols: x = used, + = saved, * = unsaved
x + EKF2_EVA_NOISE [202,302] : 0.2000
x + EKF2_EVP_GATE [203,303] : 10.0000
x + EKF2_EVP_NOISE [204,304] : 0.2000
x + EKF2_EVV_GATE [205,305] : 10.0000
x + EKF2_EVV_NOISE [206,306] : 0.2000
x + EKF2_EV_DELAY [207,307] : 150.0000
x + EKF2_EV_NOISE_MD [208,308] : 1

dagar commented 2 years ago

You need a heading source, EV vel/pos won't start until you've aligned yaw (control_status.flags.yaw_align).

Try EKF2_AID_MASK 280 with the other parameters you have above. At this point I would also use PX4 v1.13.0 or current main if you can.

arthurBricq commented 2 years ago

Ok I will try with current main and these parameters and let you know as soon as I have logs ! Thanks a lot for your help. I've been quite stuck with this since a few days..

dagar commented 2 years ago

Ok I will try with current main and these parameters and let you know as soon as I have logs ! Thanks a lot for your help. I've been quite stuck with this since a few days..

Are you on PX4 slack? If you message me there we can iterate a little faster. I'm working through VIO with another system and I want all of these common options to be well supported going forward (we have a bit of work to do).

arthurBricq commented 2 years ago

I am closing this issue since we were able to find the solution with the help of @dagar ! Heres' a quick description of the solution for those who may face the same problem.

To make sure that I had a valid heading source, in my case it was to make sure that ev_yaw was in the EKF2_AID_MASK
To set EKF2_EV_DELAYto zero, even though there is a real offset. Before it was set to 150ms, however Mavros / Mavlink was already estimating this offset properly and therefore i was compensating it twice. We could verify that the estimation was correct by looking at the field timestamp_sample of the vehicle_visual_odometrymessage.

Thanks agan !

dagar commented 2 years ago

FYI I'm proposing we change the EKF2_EV_DELAY to 0 here. https://github.com/PX4/PX4-Autopilot/pull/19860

PX4 / PX4-Autopilot