fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 06:43:34 +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 14:38:09 +02: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
EKF/covariance.cpp
+4 -3
View File
@@ -135,7 +135,8 @@ void Ekf::predictCovariance()
const float dvy_b = _state.delta_vel_bias(1);
const float dvz_b = _state.delta_vel_bias(2);
const float dt = math::constrain(_imu_sample_delayed.delta_ang_dt, 0.5f * FILTER_UPDATE_PERIOD_S, 2.0f * FILTER_UPDATE_PERIOD_S);
// Use average update interval to reduce accumulated covariance prediction errors due to small single frame dt values
const float dt = FILTER_UPDATE_PERIOD_S;
const float dt_inv = 1.0f / dt;
// convert rate of change of rate gyro bias (rad/s**2) as specified by the parameter to an expected change in delta angle (rad) since the last update
@@ -734,12 +735,12 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
for (int i = 4; i <= 6; i++) {
// NED velocity states
P(i,i) = math::constrain(P(i,i), 0.0f, P_lim[1]);
P(i,i) = math::constrain(P(i,i), 1E-6f, P_lim[1]);
}
for (int i = 7; i <= 9; i++) {
// NED position states
P(i,i) = math::constrain(P(i,i), 0.0f, P_lim[2]);
P(i,i) = math::constrain(P(i,i), 1E-6f, P_lim[2]);
}
for (int i = 10; i <= 12; i++) {