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

EKF: Add accel bias estimation fault to reporting

Browse Source
This commit is contained in:
Paul Riseborough
2017-03-10 10:14:06 +01:00
committed by Lorenz Meier
parent b51abf4aff
commit 324fe3b0c7
2 changed files with 9 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/covariance.cpp
+8 -2
View File
@@ -715,10 +715,12 @@ void Ekf::fixCovarianceErrors()
&& down_dvel_bias * _vel_pos_innov[2] < 0.0f
&& down_dvel_bias * _vel_pos_innov[5] < 0.0f);
// record the pass and force symmetry on the covariance matrix
// record the pass/fail
if (!bad_acc_bias) {
_fault_status.flags.bad_acc_bias = true;
_time_acc_bias_check = _time_last_imu;
makeSymmetrical(P,13,15);
} else {
_fault_status.flags.bad_acc_bias = true;
}
// if we have failed for 7 seconds continuously, reset the accel bias covariance
@@ -732,7 +734,11 @@ void Ekf::fixCovarianceErrors()
P[14][14] = varY;
P[15][15] = varZ;
_time_acc_bias_check = _time_last_imu;
_fault_status.flags.bad_acc_bias = false;
ECL_WARN("EKF invalid accel bias - resetting covariance");
} else {
// ensure the covariance values are symmetrical
makeSymmetrical(P,13,15);
}
}