BUG on EKF2 when the PX4Flow goes invalid

AlexisTM commented 7 years ago

Bug Report

LOG: https://logs.px4.io/plot_app?log=962b0f4e-3553-4ec2-b398-40258c9286e5

Explanation: The PX4Flow is only valid at 35cm of the floor.

To reproduce, handheld the UAV for some seconds over 50cm so we get a position then let the UAV on the ground. After some minutes, take it over the ground again and you'll see that the position is completely off.

Note: In the log, you can see the handheld parts with the accelerometer.

Anomalies in the log:

The two first handheld parts where with some rotation yet the third one (at 10') was without clear rotation.
High yawrate while one the ground (4' - 10')
Peak in LPOS.x and LPOS.y (We don't see much because of that peak)
We don't have the same peak with the distance sensor!
Velocities are high while on the ground
At 10', while handheld (almost not moving) the velocities tend to go up.

Versions:

PX4 Firmware: 1.6.5
Flow Firmware: QGC provided firmware (PX4/Flow/master)
EKF_AID_MASK set to PX4Flow

Workaround: Using the PR https://github.com/PX4/Flow/issues/96 so the PX4Flow report is always marked as valid.

Original issue: https://github.com/PX4/Flow/issues/97

ChristophTobler commented 7 years ago

@AlexisTM Thanks! I'll have a look at it. Seems to be an ekf2 issue. FYI @priseborough

ChristophTobler commented 7 years ago

Two things stand out:

The bottom distance jumps to 12m but the current distance seems ok.
The flow innovations are non-zero which probably leads to the high velocities (vx and vy)

Remark: vx and vy are scaled by 0.01 in both images

ChristophTobler commented 7 years ago

Also weird: A lot of parameters are missing in this log (e.g. EKF2_*)

AlexisTM commented 7 years ago

For the parameters, they are present on the UAV, so I guess only the non-default ones are in the log?

Note that the UAV is flying perfectly well when the PX4Flow is disconnected or with the PR https://github.com/PX4/Flow/pull/96 (with some modifications to have the PX4Flow data always valid, which leads to some drift when on the ground)

ChristophTobler commented 7 years ago

@AlexisTM

For the parameters, they are present on the UAV, so I guess only the non-default ones are in the log?

No, all of them should be

Note that the UAV is flying perfectly well when the PX4Flow is disconnected or with the PR PX4/Flow#96 (with some modifications to have the PX4Flow data always valid, which leads to some drift when on the ground)

Having flow always valid (even when it is clearly not good) is not an option.

ChristophTobler commented 7 years ago

I was able to reproduce this on master with our f450 setup with a PX4Flow and Lidar Lite v3. It seems like the positions get reset after a short while but not the velocities.

priseborough commented 7 years ago

The filter is supposed to revert back to a static mode if there is no aiding data for an extended period, but this is not happening. Also on other estimators I have used a default flow value of 0 when on the ground if flow data is invalid to constrain drift, but this is also not happening. I'll look into this some more tomorrow and submit a fix.

priseborough commented 7 years ago

I checked and the logic to assume a zero flow rate if on ground with bad flow quality is definitely in the code.

priseborough commented 7 years ago

When the flow data goes invalid, the EKF continues to dead-reckon and neither the vehicle_local_position_0.vxy_reset_counter or vehicle_local_position_0.xy_reset_counter increments during the log indicating that the apparent state reset and subsequent filter instability was caused by fusing the flow data without first resetting the position and velocity states and their corresponding covariances.

The following anomalies will be followed up tomorrow:

1) Given the vehicle was on ground and the EKF in_air flag was false, why didn't it fuse in zero flow rates when the flow sensor was invalid. 2) After a prolonged period of dead-reckoning, why didn't the EKF fall back to a constant position mode of operation and reset the position and velocity states and covariances.

@AlexisTM can you please provide me with the parameters you are using - I need the EKF2 ones which are missing from the log. You should be able to download and save them using QGC.

AlexisTM commented 7 years ago

EKF related params:

# Onboard parameters for Vehicle 1
#
# Stack: PX4 Pro
# Vehicle: Multi-Rotor
# Version: 1.6.5 dev
# Git Revision: 3098c77c9168553c
#
# Name Value Type
COM_ARM_EKF_AB  0.004999999888241291    9
COM_ARM_EKF_GB  0.000869999988935888    9
COM_ARM_EKF_HGT 1.000000000000000000    9
COM_ARM_EKF_POS 0.500000000000000000    9
COM_ARM_EKF_VEL 0.500000000000000000    9
COM_ARM_EKF_YAW 0.500000000000000000    9
COM_ARM_IMU_ACC 0.699999988079071045    9
COM_ARM_IMU_GYR 0.200000002980232239    9
COM_ARM_MIS_REQ 0   6
COM_ARM_SWISBTN 0   6
COM_ARM_WO_GPS  1   6
EKF2_ABIAS_INIT 0.200000002980232239    9
EKF2_ACC_B_NOISE    0.003000000026077032    9
EKF2_ACC_NOISE  0.349999994039535522    9
EKF2_AID_MASK   2   6
EKF2_ANGERR_INIT    0.100000001490116119    9
EKF2_ARSP_THR   0.000000000000000000    9
EKF2_ASP_DELAY  100.000000000000000000  9
EKF2_BARO_DELAY 0.000000000000000000    9
EKF2_BARO_GATE  5.000000000000000000    9
EKF2_BARO_NOISE 2.000000000000000000    9
EKF2_BETA_NOISE 0.300000011920928955    9
EKF2_DECL_TYPE  7   6
EKF2_EAS_NOISE  1.399999976158142090    9
EKF2_EVA_NOISE  0.050000000745058060    9
EKF2_EVP_NOISE  0.050000000745058060    9
EKF2_EV_DELAY   175.000000000000000000  9
EKF2_EV_GATE    5.000000000000000000    9
EKF2_EV_POS_X   0.000000000000000000    9
EKF2_EV_POS_Y   0.000000000000000000    9
EKF2_EV_POS_Z   0.000000000000000000    9
EKF2_FUSE_BETA  0   6
EKF2_GBIAS_INIT 0.100000001490116119    9
EKF2_GPS_CHECK  21  6
EKF2_GPS_DELAY  110.000000000000000000  9
EKF2_GPS_POS_X  0.000000000000000000    9
EKF2_GPS_POS_Y  0.000000000000000000    9
EKF2_GPS_POS_Z  0.000000000000000000    9
EKF2_GPS_P_GATE 5.000000000000000000    9
EKF2_GPS_P_NOISE    0.500000000000000000    9
EKF2_GPS_V_GATE 5.000000000000000000    9
EKF2_GPS_V_NOISE    0.500000000000000000    9
EKF2_GYR_B_NOISE    0.001000000047497451    9
EKF2_GYR_NOISE  0.014999999664723873    9
EKF2_HDG_GATE   2.599999904632568359    9
EKF2_HEAD_NOISE 0.300000011920928955    9
EKF2_HGT_MODE   2   6
EKF2_IMU_POS_X  0.000000000000000000    9
EKF2_IMU_POS_Y  0.000000000000000000    9
EKF2_IMU_POS_Z  0.000000000000000000    9
EKF2_MAGBIAS_ID 73225   6
EKF2_MAGBIAS_X  0.000040626870031701    9
EKF2_MAGBIAS_Y  0.000087446111137979    9
EKF2_MAGBIAS_Z  -0.000185543263796717   9
EKF2_MAGB_K 0.200000002980232239    9
EKF2_MAGB_VREF  0.000000249999999369    9
EKF2_MAG_ACCLIM 0.500000000000000000    9
EKF2_MAG_B_NOISE    0.000099999997473788    9
EKF2_MAG_DECL   0.000000000000000000    9
EKF2_MAG_DELAY  0.000000000000000000    9
EKF2_MAG_E_NOISE    0.001000000047497451    9
EKF2_MAG_GATE   3.000000000000000000    9
EKF2_MAG_NOISE  0.050000000745058060    9
EKF2_MAG_TYPE   0   6
EKF2_MAG_YAWLIM 0.250000000000000000    9
EKF2_MIN_OBS_DT 20  6
EKF2_MIN_RNG    0.300000011920928955    9
EKF2_NOAID_NOISE    10.000000000000000000   9
EKF2_OF_DELAY   5.000000000000000000    9
EKF2_OF_GATE    3.000000000000000000    9
EKF2_OF_N_MAX   0.500000000000000000    9
EKF2_OF_N_MIN   0.150000005960464478    9
EKF2_OF_POS_X   -0.059999998658895493   9
EKF2_OF_POS_Y   0.000000000000000000    9
EKF2_OF_POS_Z   0.070000000298023224    9
EKF2_OF_QMIN    1   6
EKF2_OF_RMAX    2.500000000000000000    9
EKF2_REC_RPL    0   6
EKF2_REQ_EPH    5.000000000000000000    9
EKF2_REQ_EPV    8.000000000000000000    9
EKF2_REQ_GDOP   2.500000000000000000    9
EKF2_REQ_HDRIFT 0.300000011920928955    9
EKF2_REQ_NSATS  6   6
EKF2_REQ_SACC   1.000000000000000000    9
EKF2_REQ_VDRIFT 0.500000000000000000    9
EKF2_RNG_AID    1   6
EKF2_RNG_A_HMAX 10.000000000000000000   9
EKF2_RNG_A_VMAX 1.000000000000000000    9
EKF2_RNG_DELAY  5.000000000000000000    9
EKF2_RNG_GATE   5.000000000000000000    9
EKF2_RNG_NOISE  0.100000001490116119    9
EKF2_RNG_PITCH  0.000000000000000000    9
EKF2_RNG_POS_X  -0.079999998211860657   9
EKF2_RNG_POS_Y  0.090000003576278687    9
EKF2_RNG_POS_Z  0.029999999329447746    9
EKF2_RNG_SFE    0.050000000745058060    9
EKF2_TAS_GATE   3.000000000000000000    9
EKF2_TAU_POS    0.250000000000000000    9
EKF2_TAU_VEL    0.250000000000000000    9
EKF2_TERR_GRAD  0.500000000000000000    9
EKF2_TERR_NOISE 5.000000000000000000    9
EKF2_WIND_NOISE 0.100000001490116119    9

ChristophTobler commented 7 years ago

@priseborough regarding

Given the vehicle was on ground and the EKF in_air flag was false, why didn't it fuse in zero flow rates when the flow sensor was invalid.

There is a small bug -> https://github.com/PX4/ecl/pull/334. However, doesn't fix this issue...

ChristophTobler commented 7 years ago

After a prolonged period of dead-reckoning, why didn't the EKF fall back to a constant position mode of operation and reset the position and velocity states and covariances.

I don't think it counts as dead-reckoning as there are no drops and it keeps fusing flow. If on ground and flow quality is bad, it just uses the gyro measurements and if you grab it, it uses actual pixel flow.

priseborough commented 7 years ago

The flow innovations flatline and the velocity and position diverge which is consistent with dead-reckoning.

screen shot 2017-10-11 at 7 22 10 am

priseborough commented 7 years ago

I now know why the flow is being rejected on the ground. With very bad quality, the integration interval is going to zero which is stopping flow measurements being pushed to the buffer. I'll push a fix to the ecl library.

priseborough commented 7 years ago

@ChristophTobler and @AlexisTM I've pushed the fix to https://github.com/PX4/ecl/pull/334

It flies SITL OK, but let me know how it works on your hardware.

PX4 / PX4-Autopilot

BUG on EKF2 when the PX4Flow goes invalid #8102

Bug Report