diff --git a/EKF/common.h b/EKF/common.h index c297fa6f36..1346986fd2 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -269,8 +269,11 @@ struct parameters { float vel_Tau; // velocity state correction time constant (1/sec) float pos_Tau; // postion state correction time constant (1/sec) - // state limits - float acc_bias_lim; // maximum accel bias magnitude (m/s/s) + // accel bias learning control + float acc_bias_lim; // maximum accel bias magnitude (m/s/s) + float acc_bias_learn_acc_lim; // learning is disabled if the magnitude of the IMU acceleration vector is greeater than this (m/sec**2) + float acc_bias_learn_gyr_lim; // learning is disabled if the magnitude of the IMU angular rate vector is greeater than this (rad/sec) + float acc_bias_learn_tc; // time constant used to control the decaying envelope filters applied to the accel and gyro magnitudes (sec) unsigned no_gps_timeout_max; // maximum time we allow dead reckoning while both gps position and velocity measurements are being // rejected @@ -376,8 +379,11 @@ struct parameters { vel_Tau = 0.25f; pos_Tau = 0.25f; - // state limiting + // accel bias state limiting acc_bias_lim = 0.4f; + acc_bias_learn_acc_lim = 25.0f; + acc_bias_learn_gyr_lim = 3.0f; + acc_bias_learn_tc = 0.5f; no_gps_timeout_max = 7e6; // maximum seven seconds of dead reckoning time for gps diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index e07341fe50..87cb642cdc 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -151,9 +151,19 @@ void Ekf::predictCovariance() // 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 float d_ang_bias_sig = dt * dt * math::constrain(_params.gyro_bias_p_noise, 0.0f, 1.0f); - // convert rate of change of acceerometer bias (m/s**3) as specified by the parameter to an expected change in delta velocity (m/s) since the last update + // convert rate of change of accelerometer bias (m/s**3) as specified by the parameter to an expected change in delta velocity (m/s) since the last update float d_vel_bias_sig = dt * dt * math::constrain(_params.accel_bias_p_noise, 0.0f, 1.0f); + // inhibit learning of imu acccel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities + float alpha = 1.0f - math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f); + _ang_rate_mag_filt = fmax(_imu_sample_delayed.delta_ang.norm(), alpha * _ang_rate_mag_filt); + _accel_mag_filt = fmax(_imu_sample_delayed.delta_vel.norm(), alpha * _accel_mag_filt); + if (_ang_rate_mag_filt > dt * _params.acc_bias_learn_gyr_lim || _accel_mag_filt > dt * _params.acc_bias_learn_acc_lim) { + _accel_bias_inhibit = true; + } else { + _accel_bias_inhibit = false; + } + // Don't continue to grow the earth field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned float mag_I_sig; if (_control_status.flags.mag_3D && (P[16][16] + P[17][17] + P[18][18]) < 0.1f) { @@ -371,7 +381,7 @@ void Ekf::predictCovariance() } // Don't calculate these covariance terms if IMU delta vlocity bias estimation is inhibited - if (!(_params.fusion_mode & MASK_INHIBIT_ACC_BIAS)) { + if (!(_params.fusion_mode & MASK_INHIBIT_ACC_BIAS) && !_accel_bias_inhibit) { // calculate variances and upper diagonal covariances for IMU delta velocity bias states nextP[0][13] = P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]; @@ -425,6 +435,13 @@ void Ekf::predictCovariance() nextP[i][i] += process_noise[i]; } + } else { + zeroRows(nextP,13,15); + zeroCols(nextP,13,15); + nextP[13][13] = P[14][14]; + nextP[14][14] = P[15][15]; + nextP[15][15] = P[15][15]; + } // Don't do covariance prediction on magnetic field states unless we are using 3-axis fusion diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 6b0540fcc7..35416f2943 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -196,6 +196,9 @@ bool Ekf::init(uint64_t timestamp) _fault_status.value = 0; _innov_check_fail_status.value = 0; + _accel_mag_filt = 0.0f; + _ang_rate_mag_filt = 0.0f; + return ret; } diff --git a/EKF/ekf.h b/EKF/ekf.h index ec4a72f99f..419c8e0cc9 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -321,6 +321,11 @@ private: gps_check_fail_status_u _gps_check_fail_status{}; + // variables used to inhibit accel bias learning + bool _accel_bias_inhibit; // true when the accel bias learning is being inhibited + float _accel_mag_filt; // acceleration magnitude after application of a decaying envelope filter (m/sec**2) + float _ang_rate_mag_filt; // angular rate magnitude after application of a decaying envelope filter (rad/sec) + // Terrain height state estimation float _terrain_vpos; // estimated vertical position of the terrain underneath the vehicle in local NED frame (m) float _terrain_var; // variance of terrain position estimate (m^2)