fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 08:27:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Improve covariance prediction stability (#795)

Browse Source 
* EKF: Improve covariance prediction stability

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

* EKF: Fix typo

* test: Fix initialisation test cases

Provide sufficient time for variances to stabilise and fix calculation of reference quaternion for alignment.

* test: Allow for accumulated rounding error in IMU sampling test

* test: Allow sufficient time for quaternion variances to reduce after initial alignment

* test: Increase allowance for tilt alignment delay and inertial nav errors

* test: Increase allowance for tilt alignment delay and inertial nav errors

* adpat reset velocity test

* test: update change indication file

* test: Adjust tests to handle alignment time and prediction errors

* README.md: Add documentation for change indicator test
This commit is contained in:
Paul Riseborough
2020-04-23 22:38:09 +10:00
committed by GitHub
parent 65a4ca9d65
commit 8a9d961f0d
9 changed files with 404 additions and 369 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/test_EKF_imuSampling.cpp
+5 -2
View File
@@ -76,8 +76,10 @@ TEST_P(EkfImuSamplingTest, imuSamplingAtMultipleRates)
imu_sample.delta_vel = accel * imu_sample.delta_vel_dt;
// The higher the imu rate is the more measurements we have to set before reaching the FILTER_UPDATE_PERIOD
int n_samples = 0;
for(int i = 0; i<(int)20/std::get<0>(GetParam()); ++i)
{
n_samples++;
imu_sample.time_us = _t_us;
_ekf.setIMUData(imu_sample);
_t_us += dt_us;
@@ -90,8 +92,9 @@ TEST_P(EkfImuSamplingTest, imuSamplingAtMultipleRates)
// WHEN: downsampling the imu measurement
// THEN: the delta vel should be accumulated correctly
EXPECT_TRUE(matrix::isEqual(ang_vel, imu_sample_buffered.delta_ang/imu_sample_buffered.delta_ang_dt, 1e-7f));
EXPECT_TRUE(matrix::isEqual(accel, imu_sample_buffered.delta_vel/imu_sample_buffered.delta_vel_dt, 1e-7f));
// Allow for accumulation of rounding error with each sample
EXPECT_TRUE(matrix::isEqual(ang_vel, imu_sample_buffered.delta_ang/imu_sample_buffered.delta_ang_dt, float(n_samples) * 1e-7f));
EXPECT_TRUE(matrix::isEqual(accel, imu_sample_buffered.delta_vel/imu_sample_buffered.delta_vel_dt, float(n_samples) * 1e-7f));
}
INSTANTIATE_TEST_SUITE_P(imuSamplingAtMultipleRates,