EKF2 velocity estimate wrong - v1.11-beta

[mhkabir]

Describe the bug
We are flying in indoor environments with VIO, and while testing recently, discovered that the EKF's velocity estimate significantly differed from that provided by the vision system. We don't know if this is a new issue, or whether this already existed before, since we have not flown this setup so extensively before. We also only recently plumbed the vision system's velocity estimate through to PX4, so it's easy to confirm the issue now.

EDIT: older logs (~1 mo old - upstream d7a9b123e6b84df4a7c01daaac64706c39abd3d7) don't show this velocity offset, and have lower vibration levels. I think the on-sensor DLPF was disabled between then and now. https://logs.px4.io/plot_app?log=ff62827a-7eff-45c6-b2e6-6ea22cd087c6

Expected behavior
The velocity estimate matches the one provided by the vision system, which is accurate and has been benchmarked against groundtruth. Also the vehicle was flying in a straight line, and that velocity offset wasn't actually there in real life.

Log Files and Screenshots
Here are two logs from boot illustrating the issue. The first one is without velocity fusion enabled, and the second one is with velocity fusion enabled (which doesn't make much of a difference).

1. EKF2_AID_MASK = 24 (ev pos, ev yaw): https://logs.px4.io/plot_app?log=6eb5a9cb-a4e9-4b4a-89eb-e71e11419670
2. EKF2_AID_MASK = 280 (ev pos, ev yaw, ev vel): https://logs.px4.io/plot_app?log=dcb3b53b-07d2-4db9-a118-592ef82d5cba

These are both replayable. Our branch is based on upstream 746b3124abe9df486c6d88a3d9ec0c438e63654d

First log

Accel bias estimate:

Velocity estimates:

Second log

Accel bias estimate:

Velocity estimate:

Last good log

https://logs.px4.io/plot_app?log=ff62827a-7eff-45c6-b2e6-6ea22cd087c6

Based on upstream d7a9b123e6b84df4a7c01daaac64706c39abd3d7

Drone (please complete the following information):

• 550 sized carbon quadcopter.
• Hardmounted Pixhawk 4.

[dagar]

ECL changes: PX4/PX4-ECL@230c865...47624a0

PX4/Firmware side the biggest change is the driver for your primary IMU (icm20689), which you can see from the perf counters or work_queue status.

Last good log: https://logs.px4.io/plot_app?log=ff62827a-7eff-45c6-b2e6-6ea22cd087c6

mpu6k_duplicates: 49567 events
mpu6k_reset: 0 events
mpu6k_bad_reg: 0 events
mpu6k_bad_trans: 0 events
mpu6k_read: 247571 events, 10724754us elapsed, 43.32us avg, min 40us max 78us 6.933us rms

First new log:

icm20689: DRDY interval: 5044 events, 125.01 avg, min 108us max 142us 2.316us rms
icm20689: FIFO reset: 1 events
icm20689: FIFO overflow: 0 events
icm20689: FIFO empty: 0 events
icm20689: bad transfer: 0 events
icm20689: bad register: 0 events
icm20689: transfer: 630 events, 87645us elapsed, 139.12us avg, min 132us max 296us 15.753us rms

You can also tell from the vibration metrics with the sensor side DLPF disabled (low pass with 98 Hz cutoff).

[dagar]

The accel (icm20649) calibration changed quite a bit. Notably there's no longer a huge x offset. This was also seen in #14676 (comment). I'd say there was something bogus in the old mpu6000 driver.

EDIT: typo icm20649 should have said icm20689

Old

CAL_ACC0_XOFF, -1.0432920455932617
CAL_ACC0_XSCALE, 0.9367385506629944
CAL_ACC0_YOFF, 0.07577945291996002
CAL_ACC0_YSCALE, 0.9988186955451965
CAL_ACC0_ZOFF, -0.09410905838012695
CAL_ACC0_ZSCALE, 0.9868613481521606

New

CAL_ACC0_XOFF, -0.052748680114746094
CAL_ACC0_XSCALE, 0.9987621903419495
CAL_ACC0_YOFF, 0.08501672744750977
CAL_ACC0_YSCALE, 0.9985091686248779
CAL_ACC0_ZOFF, -0.08153867721557617
CAL_ACC0_ZSCALE, 0.9868456125259399

[LorenzMeier]

@dagar I would add code to wipe the params on update. The factory calibration of the ICM20649 is so good that you're better off having none.

[mhkabir]

@RomanBapst @bresch @kamilritz @jkflying Can you have a look?

[dagar]

The integrated data in the accel x axis looks questionable in sensor_combined. This is preflight while the system is still.

This is where the estimated bias (~0.2 m/s) is coming from.

I've tried to replicate this on another fmu-v5, but no success so far. It seems like it could be a numerical problem.

[mhkabir]

Happened again on master: https://logs.px4.io/plot_app?log=6a006545-b3bf-446f-9baf-f89c5fcfb542

Same vehicle, with @dagar's new fix attempts, and a fixed companion computer fan (so lower vibrations on ground)

[bresch]

@mhkabir Your VIO system seems to provide a velocity estimate with a variance of 5 m2/s2 (std dev: 2.2m/s). A typical GPS gives velocity aiding with an accuracy of 0.5m/s.

[kamilritz]

@bresch @mhkabir Oh that is for sure not optimal. This remembers me of a bug that I wanted to fix inside the velocity-in-body-frame PR (, which is not merged yet). I am sorry for not creating a single PR for the bug fix. Can you spot the bug?

Solution

[mhkabir]

@bresch Good catch with the variances. What you're looking at is actually the position variance, thanks to the above bug. This is fishy regardless (such a large position variance is not right), and seems to be something on our side, looking into it.

[mhkabir]

There seem to have been a bunch of issues in play simultaneously here:

• Accelerometer bias estimates were exploding, which seems to have been fixed thanks to @dagar (PX4Accelerometer/PX4Gyroscope: integrated data use simple rotate_3f to avoid unnecessary floating point operations #14752 and PX4Accelerometer/PX4Gyroscope: fix calibration offset for integrated FIFO case #14765)
• Our vision position variances were wrong, found thanks to @bresch. We will fix this properly on our side longer term, for now using a fixed variance using EKF2_NOISE_MD seems to work just fine.
• Vision velocity fusion was using the wrong variances, which is fixed thanks to @kamilritz (Fix: Stop getting velocity covariance from quaternion variance #14779)

Thanks for all the help everyone!