EKF: Improve covariance prediction stability

[priseborough]

Eliminates collapse of vertical velocity state variance due to rounding errors that can occur under some operating conditions.

See issue #771 for discussion and testing against stable release version.

TODO: testing of this version

[jkflying]

Just fix the typo, these look safe as a minimum to get into 1.11 before we start looking for bigger changes to fix the underlying issue.

[dusan19]

Would it be benefitial if the filter update period was increased to 12ms to keep it as a multiple of the incoming imu samples?

[priseborough]

Would it be benefitial if the filter update period was increased to 12ms to keep it as a multiple of the incoming imu samples?

We can have significant dt jitter on IMU sampling depending on the board, so there is no benefit in selecting an integer multiple given the way the imu_down_sampler.cpp is adjusting the time cutoff to maintain the average rate at the target.

[priseborough]

We could move to a fixed down sampling ratio, eg 3:1, but it would rely on the published IMU data dt having low jitter.

[priseborough]

For example the BMI088 sensor is sampled at 800Hz which does not downsample to 250Hz with even time steps.

[priseborough]

If we went to 12, then maybe we could use the instantaneous IMU dt instead of the average dt inside the covariance prediction as the likelihood of a 1 frame IMU sample being consumed then becomes small, but I haven't tested that option. Using 12 instead of 10 will also increase timing jitter for other sensors wrt the fusion time horizon.

[priseborough]

I'm working through the CI test failures one by one adjusting the unit tests to get them to pass. Can someone else have a look at the imuSamplingAtMultipleRates test. What it is doing doesn't make sense to me, but I may be misunderstanding it.

Edit: The imuSamplingAtMultipleRates equivalence check accuracy of 1E-7f is not being met after downsampling. The achieved difference is 1.788139e-07 with a constant input angular rate of 1.0 rad/sec. I'm currently investigating to see if the use of single precision in the test harness to generate input data is contributing or whether the required check accuracy needs to be relaxed.

Edit: The unit test didn't allow for accumulated rounding error with each sample. This has now been fixed

[priseborough]

The only tests failing now are:

EkfExternalVisionTest.visionVelocityReset
EkfExternalVisionTest.visionVelocityResetWithAlignment

It's not obvious how this change is impacting these cases, so further investigation is required.

[jkflying]

@kamilritz do you have time to take a quick look at why those tests are failing?

Nevermind 🙂

[priseborough]

I've got them all passing now with exception of the change indicator which is as expected.

[kamilritz]

Cool, Another option is to increase the time before the reset and capture the state of after the reset a bit earlier, like this: kamilritz@eea0146
I would prefer this option since with this check we want to test if the reset happens to the correct value and not if we converge towards the right value.
@priseborough Are you able to update the change indication locally? Otherwise you could copy-replace it from here: kamilritz@528a235

[dagar]

We can have significant dt jitter on IMU sampling depending on the board, so there is no benefit in selecting an integer multiple given the way the imu_down_sampler.cpp is adjusting the time cutoff to maintain the average rate at the target.

It's significantly better with the new Invensense drivers in master (and the v1.11 release). Currently that's the primary IMU in the vast majority of usage. Note these still have the hard coded 4000 us integration period, but it's handled as an integer number of samples. We could easily change this to 2500 us corresponding with this ecl change.

We could move to a fixed down sampling ratio, eg 3:1, but it would rely on the published IMU data dt having low jitter.

I would like to consider that after this release. I don't see why we can't optimize the system holistically, including accommodating these fundamental differences between sensors.

[dagar]

For testing I've created PX4/PX4-Autopilot#14676 that pulls in this ECL branch and also changes the default integration period.

[priseborough]

@priseborough Are you able to update the change indication locally? Otherwise you could copy-replace it from here: kamilritz@528a235

Not with the current level of documentation for the change indication. I know you showed me once before, but I thought you were going to write a readme that specifies the steps required to generate a new file.

I forgot it was sitting here #717 (thanks @jkflying for pointing that out). I was able to regenerate it using an Ubuntu18.04VM, but have been told that using different compiler versions can be an issue. I think the steps required to update it need to be included in the project readme in the same location with instruction for submission of PR's etc.

[priseborough]

@kamilritz I pulled in your suggested test changes, but had to resolve a conflict and there are issues with unit tests failing after the rebase.

I'll have a look at the following tests that are failing tomorrow:

EkfExternalVisionTest.visionVelocityResetWithAlignment
EkfFlowTest.resetToFlowVelocityOnGround

[kamilritz]

@priseborough Great, that you could get those tests to pass. These tests seem to be really time sensitive. Maybe it is due to the fact that we compare the output velocities and not the velocities of the delayed state. I will look in to this.
And thank you very much for this nice documentation on the change indication.

[jkflying]

These are conservative changes to make things safer, and has been tested as well as it can for now. Let's get this in, so it can get wider testing as part of the 1.11 beta. With any luck this just makes things better.

[dusan19]

I tested this changes on a log that was failing due to covariance prediction instability that I recently recorded (sorry i didnt do it sooner before this PR has been merged). The results are posted here: #771 (comment)
As the plots show, this changes don't solve the problem but might make it slightly better, although the drone would still perform the fallback landing due to invalid position estimate (and crash)