EKF3: Covariance stability improvement

[priseborough]

This PR eliminated the incorrect state variances that a user encountered during a fixed wing flight without a magnetometer that resulted in multiple height and vertical velocity variance resets when that log was replayed using single precision. Replaying the log using master with double precision also eliminated the issue.

The symptons are a collapse of the vertical velocity and position variances and a resulting failure of the EKF to correct vertical velocity and position to the sensor values.

Although default use of EKF3 is new to the ArduPilot community, PX4 have been using the same mathematics in their ekf2 module estimator since 2016 and collapse of variances associated with the vertical velocity and Z axis delta velocity bias have been detected in a small number of logs with a number of mitigations introduced over the years. This potential fix to the mathematics was uncovered by Mathieu Bresciani during investigation of one such occurrence.

Whilst replay and SITL testing indicate a benefit, wider flight testing with LOG_DISARMED=1 and LOG_REPLAY=1 is required.

Here are the vertical position innovations for the original and replay using master with the --ekf-single processing option:

Here they are with replay using master with the default double precision processing - the behaviour is eliminated:

Here they are with replay using this PR with the --ekf-single precessing option:

If we look more closely at the original and replayed state variances for the original and replayed log using this PR with --ekf-single, we can see how the state variances for the vertical velocity (index 6) and position (index 9) collapse for the original log:

The current hypothesis for why this minor change to the prediction equations in the derivation improve stability is that it eliminates the presence of the qx quaternion in the rotation matrix x,x, qy in the y,y and qz in the z,z diagonals. Given that qx, qy and qz represent the cosines of a rotation vector along their respective axes, this intuitively makes sense. A rotation about Z for example should not change the projection of the Z earth axis along the Z body axis.

Even though the old and new equations are mathematically identical for a unit length quaternion, when the partial derivatives for the state transition matrix are calculated and used in the covariance prediction equation, the old equations may be resulting in higher order terms that lead to accumulation of unwanted numerical errors when single precision processing is used.

[priseborough]

This is a candidate for backport to 4.1 beta

[tridge]

@giacomo892 we'd appreciate you testing this on your Dart, thanks!

[priseborough]

Checking the GPS velocity innovations from the log shows that on replay the patch does reduce the size of the innovations when the vehicle switches to RTL, performs a turn and the yaw error becomes observable, eg:

[priseborough]

@tridge the CI build is failing due to frame size on the lowest spec boards. Is there a way to work around that?

[priseborough]

I've pushed a commit that fixes a logic bug that was preventing the XY accel biases being learned in flight unless there were large roll or pitch changes:

Original log shows no change in XY IMU accel bias (m/s/s)

Replayed log using this PR:

The gyro bias process noise reduction for EK3_GBIAS_P_NSE from 0.001 to 0.0001 has improved the gyro bias learning:

Original log shows learned gyro bias (deg/sec) is noisy

Replayed using this PR shows much smoother bias esitmates:

[tridge]

@priseborough I've run the failing test.Copter.PrecisionLoiterCompanion test with replay, and get this:


the replay log built with master is here:
http://uav.tridgell.net/EKF/3-precloiter-replay.bin

[tridge]

new formulation matches SIM2 state more closely for PN and PE:


a bit worse for PD. PD is a surprisingly long way from the sim state:

[tridge]

VN and VE are improved (lane 0 is master, lane 100 is this PR):


VD goes both ways:

[priseborough]

a bit worse for PD. PD is a surprisingly long way from the sim state:

It's actually tracking baro alt better than before see:

The error is in the simulated baro alt sensor:

[priseborough]

Overview of single precision replay logs with updated tune at commit e11a2c6

The new tune gives equivalent to silightly better tracking of GPS velociities (lower innovations) so the weighting of IMU vs GPS is approximately the same:

Bias esitimates estimates are smooth but do respond to changes in offfset when IMU temperature changes:

[priseborough]

The PrecisionLoiterCompanion test was failing due to a tight tolerance of 0.1m radially in the final position. Inspection of the log shows that this is due to a bias in the horizontal velocity innovation of 6 cm/s at the end of flight that offsets the position controller by 6 cm in esch axis.

Retuning the filter to handle a faster changing gyro bias (EK3_GBIAS_P_NSE 0.0001 -> 0.001) reduced this to 1 cm/s and the corresponding position controller offset to 1cm in each axis

However this gave a noisy gyro bias esitmate on replay of real flight logs, so a compromise tuning of 0.0003 will be adopted.

[giacomo892]

Replayed a log I had from another plane using MPU6000 (Omnibus F4 Pro) on which I had similar issues with EK3, and guess what:

Also gyro/acc biases looks much better:

Dramatic improvements for down and east innovations:

[tridge]

@priseborough here is the new log from @giacomo892 https://uav.tridgell.net/EKF/z84red%20ahrs%20issues.BIN

[priseborough]

The autotests failing currently are:

1. Plane.FlyEachFrame where after landing, the autotester does not get the 'throttle disarmed' text it is looking for. The log shows that the vehicle does disarm.

2. Copter.VibrationFailsafe where the changed filter tuning appears to have reduced the effectiveness of the current vibration compensation settings. These will need to be re-tuned.

[priseborough]

I'm still working on getting the Copter vibration failsafe working again. Changes made in the process are being pushed to https://github.com/priseborough/ardupilot/tree/pr-ekfCovariancePrediction-wip

@rmackay9 I would be grateful for some assistance with this. The current stats when running

'./Tools/autotest/autotest.py --no-clean --replay build.Copter test.CopterTests1c.VibrationFailsafe'

is that the "Vibration compensation ON" message is received, but it doesn't go into land mode.

[rmackay9]

@priseborough,

OK, the issue then is that the EKF is not losing its horizontal or vertical position estimate so the EKF failsafe isn't triggering. The purpose of this test is not actually to test the EKF failsafe so I can imagine two changes that would resolve the failing test:

1. increase the vibration levels even higher so that it does lose its position fix
2. we could modify the test so that it ensure the vehicle altitude stays within some range (using real numbers, not EKF numbers).

The 2nd choice is probably the better one.

I wrote the test so perhaps I should fix it..

[tridge]

@priseborough please also note this PR:
#18032

[priseborough]

The wind estimation when not using an airspeed sensor has been modified and the process noise and initial uncertainty retuned to give a better result for the Plane test.Plane.DeadreckoningNoAirSpeed test with equivalent wind estimation for the two estimators:

A number of factors were contributing to the poor EKF3 wind estimation performance in the test.Plane.DeadreckoningNoAirSpeed test:

1. Fusion of the default airspeed in EKF3 that is a average of ARSPD_ASPD_MAX and ARSPD_ASPD_MIN combined with a TRIM_THROTTLE that was too high caused the plane to exceed ARSPD_ASPD_MAX which degraded the airspeed estimate between turns.

2. EKF started wind speed estimation earlier in flight, whereas EKF2 started much later after the turn in guided mode had already started which gave EKF2 a better initialisation of the wind states. However this means the estimated airspeed derived from the EKF2 will be unavailable until then.

3. The process noise on for the wind states on EKF3 require larger values due to the revised covariance prediction.

4. The wind estimation for the no airspeed behaviour was tuned originally for hand launches with an into wind throw assumption whereas the test is for a downwind runway takeoff.

The wind estimation for a real flight log with airspeed sensor has also been checked on replay to check that we get acceptable state noise:

[priseborough]

Conflict fixed and rebased on master.

The tuning param changes to the IMU noise, biases and wind velocity states have been reverted. This gives noisier state estimates, but is required until the EKF can discriminate between boards using the following information:

• Lower sampling rate IMU's that are more susceptible to vibration.
• Only one sensor so there is no backup IMU and EKF to fall back on if the IMU develops a rapid bias change.
• The IMU's are temeprature calibrated so the likelihood of a rapdi change is bias is reduced.