From 20db74eca8f2f789f507177591a3a1fd6637a0ee Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 15 May 2017 16:38:17 +1000 Subject: [PATCH] Revert "EKF: Add parameter control of individual IMU axis delta velocity bias estimation" This reverts commit 9c31632e2bd8c53843dfc9dc419256607578350e. --- EKF/common.h | 2 - EKF/covariance.cpp | 138 ++++++++++++++++++--------------------------- EKF/ekf_helper.cpp | 10 +--- 3 files changed, 58 insertions(+), 92 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index daa367a145..6e86ad8d4d 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -246,7 +246,6 @@ struct parameters { float rng_gnd_clearance{0.1f}; // minimum valid value for range when on ground (m) float rng_sens_pitch{0.0f}; // Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis. float range_noise_scaler{0.0f}; // scaling from range measurement to noise (m/m) - float vehicle_variance_scaler{0.0f}; // gain applied to vehicle height variance used in calculation of height above ground observation variance // vision position fusion float ev_innov_gate{5.0f}; // vision estimator fusion innovation consistency gate size (STD) @@ -285,7 +284,6 @@ struct parameters { float acc_bias_learn_acc_lim{25.0f}; // learning is disabled if the magnitude of the IMU acceleration vector is greeater than this (m/sec**2) float acc_bias_learn_gyr_lim{3.0f}; // learning is disabled if the magnitude of the IMU angular rate vector is greeater than this (rad/sec) float acc_bias_learn_tc{0.5f}; // time constant used to control the decaying envelope filters applied to the accel and gyro magnitudes (sec) - int acc_bias_learn_mask{7}; // bitmask used to control which accelerometer axes learn an in-flight bias unsigned no_gps_timeout_max{7000000}; // maximum time we allow dead reckoning while both gps position and velocity measurements are being // rejected before attempting to reset the states to the GPS measurement (usec) diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 9052b479c4..42fe9265e2 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -47,7 +47,11 @@ void Ekf::initialiseCovariance() { - memset(P, 0, sizeof(P)); + for (unsigned i = 0; i < _k_num_states; i++) { + for (unsigned j = 0; j < _k_num_states; j++) { + P[i][j] = 0.0f; + } + } // calculate average prediction time step in sec float dt = 0.001f * (float)FILTER_UPDATE_PERIOD_MS; @@ -83,16 +87,9 @@ void Ekf::initialiseCovariance() P[12][12] = P[10][10]; // accel bias - float temp_var = sq(_params.switch_on_accel_bias * dt); - if (_params.acc_bias_learn_mask & (1<<0)) { - _prev_dvel_bias_var(0) = P[13][13] = temp_var; - } - if (_params.acc_bias_learn_mask & (1<<1)) { - _prev_dvel_bias_var(1) = P[14][14] = temp_var; - } - if (_params.acc_bias_learn_mask & (1<<2)) { - _prev_dvel_bias_var(2) = P[15][15] = temp_var; - } + _prev_dvel_bias_var(0) = P[13][13] = sq(_params.switch_on_accel_bias * dt); + _prev_dvel_bias_var(1) = P[14][14] = P[13][13]; + _prev_dvel_bias_var(2) = P[15][15] = P[13][13]; // variances for optional states @@ -214,10 +211,11 @@ void Ekf::predictCovariance() wind_vel_sig = 0.0f; } - // Construct the process noise variance diagonal for those states with a stationary process model. - // The covariance growth of the quaternion, velocity and position states is controlled separately - // by the IMU noise variance terms built into the auto generated covariance prediction code. - + // Construct the process noise variance diagonal for those states with a stationary process model + // These are kinematic states and their error growth is controlled separately by the IMU noise variances + for (unsigned i = 0; i <= 9; i++) { + process_noise[i] = 0.0f; + } // delta angle bias states process_noise[12] = process_noise[11] = process_noise[10] = sq(d_ang_bias_sig); // delta_velocity bias states @@ -401,8 +399,8 @@ void Ekf::predictCovariance() nextP[11][12] = P[11][12]; nextP[12][12] = P[12][12]; - // add gyro bias process noise variance - for (unsigned i = 10; i <= 12; i++) { + // add process noise that is not from the IMU + for (unsigned i = 0; i <= 12; i++) { nextP[i][i] += process_noise[i]; } @@ -423,13 +421,7 @@ void Ekf::predictCovariance() nextP[10][13] = P[10][13]; nextP[11][13] = P[11][13]; nextP[12][13] = P[12][13]; - - if (_params.acc_bias_learn_mask & (1<<0)) { - nextP[13][13] = P[13][13] + process_noise[13]; - } else { - nextP[13][13] = 0.0f; - } - + nextP[13][13] = P[13][13]; nextP[0][14] = P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]; nextP[1][14] = P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2; nextP[2][14] = P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2; @@ -444,13 +436,7 @@ void Ekf::predictCovariance() nextP[11][14] = P[11][14]; nextP[12][14] = P[12][14]; nextP[13][14] = P[13][14]; - - if (_params.acc_bias_learn_mask & (1<<1)) { - nextP[14][14] = P[14][14] + process_noise[14]; - } else { - nextP[14][14] = 0.0f; - } - + nextP[14][14] = P[14][14]; nextP[0][15] = P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]; nextP[1][15] = P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2; nextP[2][15] = P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2; @@ -466,11 +452,11 @@ void Ekf::predictCovariance() nextP[12][15] = P[12][15]; nextP[13][15] = P[13][15]; nextP[14][15] = P[14][15]; + nextP[15][15] = P[15][15]; - if (_params.acc_bias_learn_mask & (1<<2)) { - nextP[15][15] = P[15][15] + process_noise[15]; - } else { - nextP[15][15] = 0.0f; + // add process noise that is not from the IMU + for (unsigned i = 13; i <= 15; i++) { + nextP[i][i] += process_noise[i]; } } else { @@ -607,7 +593,7 @@ void Ekf::predictCovariance() nextP[20][21] = P[20][21]; nextP[21][21] = P[21][21]; - // add magnetic field state process noise variance + // add process noise that is not from the IMU for (unsigned i = 16; i <= 21; i++) { nextP[i][i] += process_noise[i]; } @@ -666,7 +652,7 @@ void Ekf::predictCovariance() nextP[22][23] = P[22][23]; nextP[23][23] = P[23][23]; - // add wind state process noise variance + // add process noise that is not from the IMU for (unsigned i = 22; i <= 23; i++) { nextP[i][i] += process_noise[i]; } @@ -751,56 +737,44 @@ void Ekf::fixCovarianceErrors() zeroCols(P,13,15); } else { // constrain variances - for (uint8_t i=0; i < 3; i++) { - uint8_t state_index = 13 + i; - if (_params.acc_bias_learn_mask & (1< dVel_bias_lim - && down_dvel_bias * _vel_pos_innov[2] < 0.0f - && down_dvel_bias * _vel_pos_innov[5] < 0.0f); + // check that the vertical componenent of accel bias is consistent with both the vertical position and velocity innovation + bool bad_acc_bias = (fabsf(down_dvel_bias) > dVel_bias_lim + && down_dvel_bias * _vel_pos_innov[2] < 0.0f + && down_dvel_bias * _vel_pos_innov[5] < 0.0f); - // record the pass/fail - if (!bad_acc_bias) { - _fault_status.flags.bad_acc_bias = false; - _time_acc_bias_check = _time_last_imu; - } else { - _fault_status.flags.bad_acc_bias = true; - } - - // if we have failed for 7 seconds continuously, reset the accel bias covariances to fix bad conditioning of - // the covariance matrix but preserve the variances (diagonals) to allow bias learning to continue - if (_time_last_imu - _time_acc_bias_check > 7E6) { - float varX = P[13][13]; - float varY = P[14][14]; - float varZ = P[15][15]; - zeroRows(P,13,15); - zeroCols(P,13,15); - P[13][13] = varX; - 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); - } + // record the pass/fail + if (!bad_acc_bias) { + _fault_status.flags.bad_acc_bias = false; + _time_acc_bias_check = _time_last_imu; + } else { + _fault_status.flags.bad_acc_bias = true; + } + // if we have failed for 7 seconds continuously, reset the accel bias covariances to fix bad conditioning of + // the covariance matrix but preserve the variances (diagonals) to allow bias learning to continue + if (_time_last_imu - _time_acc_bias_check > 7E6) { + float varX = P[13][13]; + float varY = P[14][14]; + float varZ = P[15][15]; + zeroRows(P,13,15); + zeroCols(P,13,15); + P[13][13] = varX; + 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); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 8e58300c83..27c432a471 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -549,14 +549,8 @@ void Ekf::constrainStates() _state.gyro_bias(i) = math::constrain(_state.gyro_bias(i), -0.349066f * _dt_ekf_avg, 0.349066f * _dt_ekf_avg); } - float del_vel_bias_lim = _params.acc_bias_lim * _dt_ekf_avg; - for (uint8_t i=0; i < 3; i++) { - uint8_t state_index = 13 + i; - if (_params.acc_bias_learn_mask & (1<