From 063533afae07b7f99aca1b1ad245fe7cda575d11 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 26 Oct 2017 10:41:39 +1100 Subject: [PATCH] EKF: Add method to enable the IMU bias states to be reset externally --- EKF/covariance.cpp | 3 +++ EKF/ekf.h | 9 +++++++++ EKF/ekf_helper.cpp | 30 ++++++++++++++++++++++++++++++ EKF/estimator_interface.h | 7 +++++++ 4 files changed, 49 insertions(+) diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index b21a402ce7..33bfbc2fe7 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -91,6 +91,9 @@ void Ekf::initialiseCovariance() _prev_dvel_bias_var(1) = P[14][14] = P[13][13]; _prev_dvel_bias_var(2) = P[15][15] = P[13][13]; + // record IMU bias state covariance reset time - used to prevent resets being performed too often + _last_imu_bias_cov_reset_us = _imu_sample_delayed.time_us; + // variances for optional states // earth frame and body frame magnetic field diff --git a/EKF/ekf.h b/EKF/ekf.h index 080b296b73..45eaf5b014 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -145,6 +145,13 @@ public: */ void get_ekf_ctrl_limits(float *vxy_max, bool *limit_hagl); + /* + Reset all IMU bias states and covariances to initial alignment values. + Use when the IMU sensor has changed. + Returns true if reset performed, false if rejected due to less than 10 seconds lapsed since last reset. + */ + bool reset_imu_bias(); + void get_vel_var(Vector3f &vel_var); void get_pos_var(Vector3f &pos_var); @@ -282,6 +289,8 @@ private: uint64_t _time_acc_bias_check{0}; ///< last time the accel bias check passed (uSec) uint64_t _delta_time_baro_us{0}; ///< delta time between two consecutive delayed baro samples from the buffer (uSec) + uint64_t _last_imu_bias_cov_reset_us{0}; ///< time the last reset of IMU delta angle and velocity state covariances was performed (uSec) + Vector3f _earth_rate_NED; ///< earth rotation vector (NED) in rad/s Dcmf _R_to_earth; ///< transformation matrix from body frame to earth frame from last EKF predition diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index de27c0db40..4c49905671 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1039,6 +1039,36 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, bool *limit_hagl) } +bool Ekf::reset_imu_bias() +{ + if (_imu_sample_delayed.time_us - _last_imu_bias_cov_reset_us < 10 * (1000 * 1000)) { + return false; + + } + + // Zero the delta angle and delta velocity bias states + _state.gyro_bias.zero(); + _state.accel_bias.zero(); + + // Zero the corresponding covariances + zeroCols(P, 10, 15); + zeroRows(P, 10, 15); + + // Set the corresponding variances to the values use for initial alignment + float dt = 0.001f * (float)FILTER_UPDATE_PERIOD_MS; + P[12][12] = P[11][11] = P[10][10] = sq(_params.switch_on_gyro_bias * dt); + P[15][15] = P[14][14] = P[13][13] = sq(_params.switch_on_accel_bias * dt); + _last_imu_bias_cov_reset_us = _imu_sample_delayed.time_us; + + // Set previous frame values + _prev_dvel_bias_var(0) = P[13][13]; + _prev_dvel_bias_var(1) = P[14][14]; + _prev_dvel_bias_var(2) = P[15][15]; + + return true; + +} + // get EKF innovation consistency check status information comprising of: // status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check // Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold. diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index f337da9685..bbdb63ca91 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -192,6 +192,13 @@ public: // set vehicle landed status data void set_in_air_status(bool in_air) {_control_status.flags.in_air = in_air;} + /* + Reset all IMU bias states and covariances to initial alignment values. + Use when the IMU sensor has changed. + Returns true if reset performed, false if rejected due to less than 10 seconds lapsed since last reset. + */ + virtual bool reset_imu_bias() = 0; + // get vehicle landed status data bool get_in_air_status() {return _control_status.flags.in_air;}