AP_NavEKF3 Prevent rejection of airspeed when dead reckoning

[priseborough]

When flying a plane without GPS, gusts or transiting through wind shear layers can cause the airspeed measurement to fail consistency checks and be rejected which can result in a fall-back to a non-navigating state. This patch correct this behaviour by clipping the innovations instead of rejecting the airspeed measurement when the EKF is dead reckoning.

[tridge]

@priseborough looks good to me. It is marked as WIP, do you want me to do some testing? I double deliberately adjust ARSPD_RATIO on my quadplane or SITL to test

[priseborough]

I'm investigating a potential issue arising from SITL testing where setting ARSPD_RATIO to 1.5 during dead reckoning to simulate partially blocked airspeed sensor resulted in a larger than expected loss of navigation performance after a few minutes. I may have to increase the observation noise variance in addition to clipping the innovation.

[priseborough]

I completed the following SITL test cases:

1. test of rapid 5 m/s change in wind speed when dead reckoning which simulates climb or descent through a wind shear layer.

The following map shows the period of time from disabling of GPS until wind speed change is reverted. The estimated position drifts upwind at a rate consistent with the change in wind speed.

2023-06-25 09:39:40.379 SIM_GPS_DISABLE 0.000000 -> 1.000000
2023-06-25 09:40:56.139 SIM_WIND_SPD 0.000000 -> 5.000000
2023-06-25 09:42:33.379 SIM_WIND_SPD 5.000000 -> 0.000000

Airspeed innovations spike and then decay to zero

2. test of a rapid reduction in airspeed sensor scale factor when dead reckoning which simulates a sudden partial blockage.

The first test with a zero wind speed behaved as expected.

2023-06-25 09:45:46.639 ARSPD_RATIO 2.000000 -> 1.500000
2023-06-25 09:48:21.639 ARSPD_RATIO 1.500000 -> 2.000000

The GPS was then re-enabled, wind speed increased and GPS disabled after allowing sufficient time for the wind speed to be learned

2023-06-25 09:50:27.879 SIM_GPS_DISABLE 1.000000 -> 0.000000
2023-06-25 09:51:26.879 SIM_WIND_SPD 0.000000 -> 5.000000
2023-06-25 09:55:42.879 SIM_GPS_DISABLE 0.000000 -> 1.000000

A partial blockage of the airspeed sensor was then simulated

2023-06-25 09:57:11.879 ARSPD_RATIO 2.000000 -> 1.500000
2023-06-25 10:00:53.139 ARSPD_RATIO 1.500000 -> 2.000000

The effect of a scale factor error on teh wind relative speed is to skew the trajectory.

When the partial blockage was removed, the navigation performance was poor with velocity errors that prevented the vehicle from following a circular orbit.

Here is the EKF's estimated velocity vs the GPS velocity in the north and east directions from when the airspeed scale factor was reduced, then restored then through to when GPS was re-enabled.

The speed error is within expected bounds given the airspeed estimation error of 4 m/s

There is a build up in yaw and course angle error.

This testing shows there is a EKF3 vulnerability to the combination of GPS loss, wind and large airspeed sensor errors which result in a yaw angle offset.

The following patches were evaluated to see if yaw and ground course angle errors could be reduced.

1. Rework of the patch so that instead of clipping the airspeed innovation at the gate limit when the test ratio is >1, the i innovation variance is scaled up instead so that the Kalman gain is consistent with a test ratio of 1. This gives the same effect as before with respect to the change in states for a given observation but has the additional effect that the covariance matrix update reflects the reduce confidence of the measurement. See priseborough@c80dfeb
2. Use a smaller airspeed innovation gate size when dead reckoning. See priseborough@0eac1d7
3. Use s smaller sideslip angle observation noise when dead reckoning. See priseborough@b400939

These changes were evaluated by replaying the SITL test log.

Patch 1. did not noticeably reduce ground course angle error when dead reckoning in wind with the -25% airspeed scale factor error.

Patches 1 and 2 slightly reduced ground course angle error:

Patches 1,2 and 3 slightly reduced ground course angle error:

Looking at yaw angle estimation error across all tests:

Patch 1 only makes no difference because the airspeed innovation test limit is not being exceeded.

Patches 1 & 2 significantly reduced yaw angle estimation error

There was some transient wind speed estimation error

Patches 1, 2 & 3 further reduced yaw angle estimation error.

I'll continue the testing and provide updated patches if required. We will need to accept the inherent limitations associated with use of airspeed sensor relative navigation and importance of sensor calibration.

[priseborough]

The ability to handle a partial blockage of the airspeed sensor when dead reckoning with 7 m/s of wind was evaluated with SITL simulation of current upstream master and then the log was replayed using the following commits:

1. priseborough@0444436
2. priseborough@c80dfeb
3. priseborough@0eac1d7
4. priseborough@b400939

2023-06-25 12:34:02.199 SIM_WIND_SPD 0.000000 -> 7.000000
2023-06-25 12:40:12.959 SIM_GPS_DISABLE 0.000000 -> 1.000000
2023-06-25 12:42:01.459 ARSPD_RATIO 2.000000 -> 1.500000
2023-06-25 12:45:19.699 ARSPD_RATIO 1.500000 -> 2.000000
2023-06-25 12:45:33.959 SIM_GPS_DISABLE 1.000000 -> 0.000000

Commit 1 - there was not change to velocity or yaw estimation accuracy

Commits 1 and 2 - same as above, no change

Commits 1,2 and 3 - small change to velocity and yaw esitmates when airspeed sensor partial blockage is removed.

Commits 1,2,3 and 4 - Using a smaller process noise for sideslip fusion when dead reckoning slightly increased estimation errors when the airspeed partial blockage/scale factor error was introduced:

The summary from SITL testing is that:

1. The sensitivity to airspeed scale factor error when dead reckoning in wind is an inherent attribute of the EKF's method of dead reckoning and not something introduced by this PR.
2. Commit 1. was effective in preventing the EKF rejecting airspeed measurements when dead reckoning.

The commit 2. upscaling scaling of the airspeed innovation variance when the airspeed observation is theoretically a good thing to do instead of just clipping the innovation, because the covariance update operation will then factor in the likely reduced accuracy of the airspeed measurement.

The commit 3. use of a smaller innovation gate for airspeed did slightly reduce yaw angle estimation error with replay of the SITL test log described in the earlier comment so it is worth including.