dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: Increase sensitivity and add tuning of bad accel checks

This commit is contained in:
Paul Riseborough 2017-04-25 11:11:59 +10:00 committed by Lorenz Meier
parent e35921534d
commit 5ee0ed5a0d
2 changed files with 11 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
EKF/common.h
View File

@ -177,7 +177,6 @@ struct dragSample {
// bad accelerometer detection and mitigation
#define BADACC_PROBATION 10E6 // Number of usec that accel data declared bad must continuously pass checks to be declared good
#define BADACC_HGT_RESET 1E6 // Number of usec that accel data must continuously fail checks to trigger a height reset
#define BADACC_BIAS_PNOISE 4.9f // The delta velocity process noise is set to this when accel data is declared bad (m/s**2)
struct parameters {
@ -296,6 +295,10 @@ struct parameters {
float bcoef_x{25.0f}; // ballistic coefficient along the X-axis (kg/m**2)
float bcoef_y{25.0f}; // ballistic coefficient along the Y-axis (kg/m**2)
// control of accel error detection and mitigation (IMU clipping)
float vert_innov_test_lim{4.5f}; // Number of standard deviations allowed before the combined vertical velocity and position test is declared as failed
int bad_acc_reset_delay_us{500000}; // Continuous time that the vertical position and velocity innovation test must fail before the states are reset (usec)
};
struct stateSample {

13
EKF/control.cpp
View File

@ -458,13 +458,14 @@ void Ekf::controlHeightSensorTimeouts()
* source failed if we have switched.
*/
// check for inertial sensing errors as evidenced by the vertical innovations having the same sign and not stale
// Check for IMU accelerometer vibration induced clipping as evidenced by the vertical innovations being positive and not stale.
// Clipping causes the average accel reading to move towards zero which makes the INS think it is falling and produces positive vertical innovations
float var_product_lim = sq(_params.vert_innov_test_lim) * sq(_params.vert_innov_test_lim);
bool bad_vert_accel = (_control_status.flags.baro_hgt && // we can only run this check if vertical position and velocity observations are indepedant
(_vel_pos_innov[5] * _vel_pos_innov[2] > 0.0f) && // vertical position and velocity sensors are in agreement
(sq(_vel_pos_innov[5] * _vel_pos_innov[2]) > var_product_lim * (_vel_pos_innov_var[5] * _vel_pos_innov_var[2])) && // vertical position and velocity sensors are in agreement that we have a significant error
(_vel_pos_innov[2] > 0.0f) && // positive innovation indicates that the inertial nav thinks it is falling
((_imu_sample_delayed.time_us - _baro_sample_delayed.time_us) < 2 * BARO_MAX_INTERVAL) && // vertical position data is fresh
((_imu_sample_delayed.time_us - _gps_sample_delayed.time_us) < 2 * GPS_MAX_INTERVAL) && // vertical velocity data is freshs
_vel_pos_test_ratio[2] > 1.0f && // vertical velocty innovations have failed innovation consistency checks
_vel_pos_test_ratio[5] > 1.0f); // vertical position innovations have failed innovation consistency checks
((_imu_sample_delayed.time_us - _gps_sample_delayed.time_us) < 2 * GPS_MAX_INTERVAL)); // vertical velocity data is fresh
// record time of last bad vert accel
if (bad_vert_accel) {
@ -482,7 +483,7 @@ void Ekf::controlHeightSensorTimeouts()
}
// check if height is continuously failing becasue of accel errors
bool continuous_bad_accel_hgt = ((_time_last_imu - _time_good_vert_accel) > BADACC_HGT_RESET);
bool continuous_bad_accel_hgt = ((_time_last_imu - _time_good_vert_accel) > (unsigned)_params.bad_acc_reset_delay_us);
// check if height has been inertial deadreckoning for too long
bool hgt_fusion_timeout = ((_time_last_imu - _time_last_hgt_fuse) > 5e6);