Copter: EKF3 still initialising pre-arm message when trying to arm with no compass (attempt2)

[rmackay9]

This is a follow-up to issue #15967 which may have actually been two separate issues one of which is now fixed.

The issue is that "EKF3 still initialising" may appear if both the compass and GPS are disabled. I was able to reproduce the issue (once) on a CubeBlack by doing this:

• loaded master onto the board
• enabled EKF3 (EK3_ENABLE = 1, EK2_ENABLE = 0, AHRS_EKF_TYPE = 3)
• EK3_SRC1_POSXY 0 (None)
• EK3_SRC1_VELXY 0 (None)
• EK3_SRC1_VELZ 0 (None)
• EK3_SRC1_YAW 0 (None)
• COMPASS_USE/2/3 = 0

I've placed a replay log here: https://www.dropbox.com/s/g6pn8ijcg8jof5f/randy-ekf3-still-initialising-master-18Dec2020.bin?dl=0

FYI @andyp1per

[andyp1per]

@priseborough I can also still reproduce this. Happy to try and get a replay log if it will help.

[anbello]

Here a log with master ArduCopter V4.1.0-dev (4dc5162) on OmnibusNanoV6 LOG_REPLAY=1
It is visible both the EKF3 still initialising pre-arm message and YAW drifting

https://drive.google.com/file/d/1McLge3sId_26OQb7iSStFRMaKVcw70Bg/view?usp=sharing

[tridge]

maybe we need this code in EKF3?
https://github.com/ardupilot/ardupilot/blob/master/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp#L610

[andyp1per]

Looks like this fixes it for me! I have created a PR.

[priseborough]

@rmackay9, further investigation of the log you posted shows the issue is that the vertical position innovations for the second EKF instance go outside the 1m limit set for on-ground operation. The inclusion of the patch from #16314 does successfully reset this behaviour, see graph from replay below comparing vertical position (baro innovation) and estimted Z accel bias for the original [1] and replayed [101] EKF data.

There is step in the baro before which does coincide with the change to BARO1_GND_PRES and a arming event. This does cause a jump in innovations which should be avoided, but this is not the underlying issue.

I'll keep picking away at that log.

[andyp1per]

So hopefully here is an example replay log with yaw drift:
1 01-01-1970 00-00-01.zip

I also videoed it:
https://youtu.be/II8UmFjLNPM
Please ignore the de-sync and crash at the end - this is because of the high logging rate and sharing of the flash SPI bus

@rmackay9 @priseborough FYI

[andyp1per]

@rmackay9 @priseborough here is a replay log with EKF3 and severe yaw draft on a copter without a compass. The copter has rotated by a full 180 by the end of the log. It was a little difficult to keep it airborne due to DPS310 baro issues:

ek3.zip

I also tried to do an EKF2 log for comparison, not sure its as good:

ek2.zip

[priseborough]

@rmackay9 I've been through the log you shared with this issue and also replayed it and can't find an internal issue with the EKF for IMU0. The XKF4.SS is 65701 or 0001 0000 0000 1010 0101 which means the following solution status flags are true which is consistent with the sensors being used.

    bool attitude           : 1; // 0 - true if attitude estimate is valid
    bool vert_vel           : 1; // 2 - true if the vertical velocity estimate is valid
    bool vert_pos           : 1; // 5 - true if the vertical position estimate is valid
    bool const_pos_mode     : 1; // 7 - true if we are in const position mode
     bool initalized         : 1; // 16 - true if the EKF has ever been healthy

However the EKF using the second IMU does drop to a solution status of zero due to the vertical position innovation exceeding check limits:

Replaying with master shows that the error in the EKF using the second IMU does disappear after the automatic reset, so there have been changes made since the log was generated that improve this behaviour, eg:

Interestingly the second IMU has a significant Z accel bias error of 0.4 m/s/s, eg:

Including the patch from #16842 makes no difference to the replay using master.

[andyp1per]

My issues appear to be fixed by #16842

[priseborough]

It looks like the source issue is the on ground not moving check used to constrain yaw drift when on ground and enable learning of the yaw bias before flight is too stringent for the noisier IMU's. I'm now in the process of enabling logging of the check metrics when required for each EKF instance and adding a parameter enabling it to be adjusted if necessary.

[priseborough]

I've pushed a couple of additional patches to #16842 which solve the failure of the second EKF in @rmackay9 's log to detect the on ground not moving condition. This adds a parameter enabling the sensitivity fo the test to be adjusted. @rmackay9 can you check if #16842 resolves the issue with your board.

[priseborough]

@andyp1per Your yaw spin issue was caused by the EKF trying to learn gyro biases when fusing in the 'fake' position measurements that are used to stop roll/pitch angle error buildup. Any axis aligned with the gravity vector is inherently unobservable unless an external source of horizontal position or velocity is being used . I've added a couple fo patches that fix this behaviour.

Here are obtained by replaying your log at commit dff864a

The replayed data [100] vs the original data [1] shows:

The EKF yaw angle is consistent with the observed yaw spin:

The estimated yaw gyro bias doesn't keep building up:

The X and Y gyro bias estimates look sensible:

[andyp1per]

Oh very nice @priseborough - thanks so much!

[priseborough]

Fixed by #16842