[Bug] EKF2 local position estimate Vz and z are inconsistent (opposite directions)

[hmloww]

Describe the bug

Hi devs

Drone is a small 280g customized quad with 3" props.
VIO, no GNSS, no mag, baro not used. Indoor flight.

When i move left and right (roll) at 3m/s, the drone sinks and climbs respectively, without being commanded.
When i move forward and backward at 3m/s, the drone sinks and climbs respectively though the effect is not as severe.

From the position and velocity controller point of view, the drone thinks it is behaving correctly because the local position estimate/vz is tracking the local position estimate setpoint/vz, and are attempting to "correct" (in reality, it is moving in the opposite direction) for the change in local position estimate/z.

I am able to confirm that my VIO reported vz and z reflects real world observation because both are moving in the same direction and agrees with my visual observation.

Overlaying the VIO module's reported vz and z onto PX4's log shows that on their own, VIO's vz and z are consistent, and the local position estimate/vz and z does not match up quite well.

What i have tried, but does not work:
-Set EKF2_EV_NOISE_MD to take EV noise parameters and increased EKF2_ACC_NOISE to as high as 0.1 to reduce IMU weightage
-Looking at the noise/vibration (concluded that the vibration levels are actually rather low) picked up by the IMU
-Increased EKF2_ACC_B_NOISE to maximum
-Repeated the tests and operating the drone when the temp of the IMU is within 5 deg C of when the IMU when calibrated
-Inhibited accel and gyro bias estimates

To Reproduce

-Drone switched on
-Switch to Position flight mode
-Take off
-Move forward (full sticks) for 2s (ie. 3m/s)
-Move backward for 2s
-Yaw right 90 degrees
-Move left (full sticks) for 2s
-Move right for 2s

The uncommanded increase and decrease in altitude will be apparent.
The problem is not as pronounced when the speed of the drone is lower (< 3m/s)

Expected behavior

There should be no uncommanded changes in altitude.

The local position estimate vz/ and local position estimate/z should be consistent and not move in different directions.

Screenshot / Media

This is for the case of roll (moving left and right)
vel_ch_wrt_par_z(m/s) and T_ch_wrt_par_z(m) are VIO reported vz and z respectively.

You can see that:
-VIO's vz is positive while local position estimate/vz is negative at some point
-local position estimate/vz is negative (or near 0) but local position estimate/z has a positive rate of change (inconsistent)

This is for the case of pitch (moving forward and back), which is not as bad Estimated acc bias changes significantly during the roll lefts and roll rights. But im not sure what to make of it.

Flight Log

https://review.px4.io/plot_app?log=60d3ae13-31ed-4390-9ad6-a9e0ad230c64

Software Version

HW arch: MODALAI_VOXL2
PX4 git-hash: bb09130
PX4 version: 1.14.0 0 (17694720)
Vendor version: 2.0.43 0 (33565440)
OS: Linux
OS version: Release 4.19.125 (68386303)
Build datetime: Aug 1 2023 17:34:01
Build uri: localhost
Build variant: default
Toolchain: GNU GCC, 7.5.0
PX4GUID: 100a41444f4d5649414c324c584f30303030
UNKNOWN MCU

Flight controller

ModalAI VOXL2

Vehicle type

Multicopter

How are the different components wired up (including port information)

No response

Additional context

No response

[dagar]

I'll have to take a closer look at this later, but have you made any other customizations? Are these timeout parameters?

Why is the vision velocity gate (EKF2_EVV_GATE) so large (25 standard deviations)?

[dagar]

What's the closet commit to PX4 main?

[dagar]

If you're able to quickly test could you try raising the EVV/EVP noise a bit, and lowering the EVV/EVP gates to something reasonable (eg 5)? Leave the EKF2_ACC_B_NOISE and IMU bias estimation default.

You can also set EKF2_EV_QMIN to something sane as a precaution (maybe 10?), although I don't think it would have mattered here.

[hmloww]

What's the closet commit to PX4 main?

I believe it would be this
https://github.com/modalai/px4-firmware/tree/bb09130cee925bdbbc4425b45c86d5a32c2586f1

[hmloww]

If you're able to quickly test could you try raising the EVV/EVP noise a bit, and lowering the EVV/EVP gates to something reasonable (eg 5)? Leave the EKF2_ACC_B_NOISE and IMU bias estimation default.

You can also set EKF2_EV_QMIN to something sane as a precaution (maybe 10?), although I don't think it would have mattered here.

Ok I'll give it a shot tmr (it's nighttime now over here).
But wouldn't raising EVV/EVP noise end up giving less weightage on VIO measurements when it is the one that is seemingly more accurate?

[hmloww]

Leave the EKF2_ACC_B_NOISE and IMU bias estimation default.

Which parameter are you referring to for "IMU bias estimation default"?
Is it EKF2_IMU_CTRL?

[hmloww]

Tried the following

EKF2_EVV_GATE 3
EKF2_EVV_NOISE 0.1
EKF2_EVP_GATE 5
EKF2_EVP_NOISE 0.1

Doesnt seem to have made any diff; local position Vz and z gradient are still in different directions during full roll left and right

Appended new log here:
https://review.px4.io/plot_app?log=81e095da-f170-45e6-8489-f3e13766dbba

[bresch]

I am able to confirm that my VIO reported vz and z reflects real world observation because both are moving in the same direction and agrees with my visual observation.

If I plot the reported vz (rotated to NED) and the derivative of z from VIO, they don't look consistent at all. Also, you can see that EKF2 follows the reported velocity quite well and this is why it's not consistent with the VIO altitude.

This then looks to be an issue of your VIO sensor and not EKF2.