diff --git a/msg/SensorCombined.msg b/msg/SensorCombined.msg index 837c7a1fac..561c491bba 100644 --- a/msg/SensorCombined.msg +++ b/msg/SensorCombined.msg @@ -21,5 +21,5 @@ uint8 CLIPPING_Z = 4 uint8 accelerometer_clipping # bitfield indicating if there was any accelerometer clipping (per axis) during the integration time frame uint8 gyro_clipping # bitfield indicating if there was any gyro clipping (per axis) during the integration time frame -uint8 accel_calibration_count # Calibration changed counter. Monotonically increases whenever accelermeter calibration changes. +uint8 accel_calibration_count # Calibration changed counter. Monotonically increases whenever accelerometer calibration changes. uint8 gyro_calibration_count # Calibration changed counter. Monotonically increases whenever rate gyro calibration changes. diff --git a/msg/VehicleImu.msg b/msg/VehicleImu.msg index a71bb7a01f..e1ca851f9d 100644 --- a/msg/VehicleImu.msg +++ b/msg/VehicleImu.msg @@ -19,5 +19,5 @@ uint8 CLIPPING_Z = 4 uint8 delta_angle_clipping # bitfield indicating if there was any gyro clipping (per axis) during the integration time frame uint8 delta_velocity_clipping # bitfield indicating if there was any accelerometer clipping (per axis) during the integration time frame -uint8 accel_calibration_count # Calibration changed counter. Monotonically increases whenever accelermeter calibration changes. +uint8 accel_calibration_count # Calibration changed counter. Monotonically increases whenever accelerometer calibration changes. uint8 gyro_calibration_count # Calibration changed counter. Monotonically increases whenever rate gyro calibration changes. diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index cf822e5fa6..5cca4cce24 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -192,6 +192,8 @@ struct imuSample { float delta_ang_dt{}; ///< delta angle integration period (sec) float delta_vel_dt{}; ///< delta velocity integration period (sec) bool delta_vel_clipping[3] {}; ///< true (per axis) if this sample contained any accelerometer clipping + bool accel_reset{false}; ///< accelerometer calibration or sensor changed + bool gyro_reset{false}; ///< gyroscope calibration or sensor changed }; struct gpsSample { diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 863dbfc9ab..8e651d6433 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -54,11 +54,6 @@ void Ekf::initialiseCovariance() { P.zero(); - _delta_angle_bias_var_accum.setZero(); - _delta_vel_bias_var_accum.setZero(); - - const float dt = _dt_ekf_avg; - resetQuatCov(); // velocity @@ -80,15 +75,9 @@ void Ekf::initialiseCovariance() P(9,9) = sq(fmaxf(_params.baro_noise, 0.01f)); } - // gyro bias - _prev_delta_ang_bias_var(0) = P(10,10) = sq(_params.switch_on_gyro_bias * dt); - _prev_delta_ang_bias_var(1) = P(11,11) = P(10,10); - _prev_delta_ang_bias_var(2) = P(12,12) = P(10,10); + resetGyroBias(); - // accel bias - _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); + resetAccelBias(); resetMagCov(); diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 483c7355fd..97585246fb 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -159,6 +159,18 @@ bool Ekf::update() // TODO: explicitly pop at desired time horizon const imuSample imu_sample_delayed = _imu_buffer.get_oldest(); + // reset accel bias if calibration or sensor changed + if (imu_sample_delayed.accel_reset) { + ECL_INFO("accel reset (delayed time horizon)"); + resetAccelBias(); + } + + // reset gyro bias if calibration or sensor changed + if (imu_sample_delayed.gyro_reset) { + ECL_INFO("gyro reset (delayed time horizon)"); + resetGyroBias(); + } + // perform state and covariance prediction for the main filter predictCovariance(imu_sample_delayed); predictState(imu_sample_delayed); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 42cea5ac9c..0ec69b513e 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -247,9 +247,8 @@ public: void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const; // Reset all IMU bias states and covariances to initial alignment values. - void resetImuBias(); - void resetGyroBias(); - void resetAccelBias(); + void resetGyroBias() override final; + void resetAccelBias() override final; Vector3f getVelocityVariance() const { return P.slice<3, 3>(4, 4).diag(); }; diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index ffca0b2a93..96e8579bb9 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -576,12 +576,6 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl } } -void Ekf::resetImuBias() -{ - resetGyroBias(); - resetAccelBias(); -} - void Ekf::resetGyroBias() { // Zero the delta angle and delta velocity bias states @@ -590,6 +584,11 @@ void Ekf::resetGyroBias() // Zero the corresponding covariances and set // variances to the values use for initial alignment P.uncorrelateCovarianceSetVariance<3>(10, sq(_params.switch_on_gyro_bias * _dt_ekf_avg)); + + // Set previous frame values + _prev_delta_ang_bias_var = P.slice<3, 3>(10, 10).diag(); + + _delta_angle_bias_var_accum.zero(); } void Ekf::resetAccelBias() @@ -603,6 +602,8 @@ void Ekf::resetAccelBias() // Set previous frame values _prev_dvel_bias_var = P.slice<3, 3>(13, 13).diag(); + + _delta_vel_bias_var_accum.zero(); } // get EKF innovation consistency check status information comprising of: diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index ea5f66e38e..98baab9d39 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -67,6 +67,18 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample) _time_latest_us = imu_sample.time_us; + // reset accel bias immediately if calibration or sensor changed (new IMU + bias used by output predictor) + if (imu_sample.accel_reset) { + ECL_INFO("accel reset"); + resetAccelBias(); + } + + // reset gyro bias immediately if calibration or sensor changed (new IMU + bias used by output predictor) + if (imu_sample.gyro_reset) { + ECL_INFO("gyro reset"); + resetGyroBias(); + } + // the output observer always runs _output_predictor.calculateOutputStates(imu_sample.time_us, imu_sample.delta_ang, imu_sample.delta_ang_dt, imu_sample.delta_vel, imu_sample.delta_vel_dt); diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 7a08d3329b..8e988cb639 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -237,6 +237,9 @@ public: // Getter for the average EKF update period in s float get_dt_ekf_avg() const { return _dt_ekf_avg; } + virtual void resetGyroBias() = 0; + virtual void resetAccelBias() = 0; + // Getters for samples on the delayed time horizon const imuSample &get_imu_sample_delayed() const { return _imu_buffer.get_oldest(); } const uint64_t &time_delayed_us() const { return _time_delayed_us; } diff --git a/src/modules/ekf2/EKF/imu_down_sampler.cpp b/src/modules/ekf2/EKF/imu_down_sampler.cpp index 02f43a12f5..93f9ef278c 100644 --- a/src/modules/ekf2/EKF/imu_down_sampler.cpp +++ b/src/modules/ekf2/EKF/imu_down_sampler.cpp @@ -21,6 +21,8 @@ bool ImuDownSampler::update(const imuSample &imu_sample_new) _imu_down_sampled.delta_vel_clipping[0] |= imu_sample_new.delta_vel_clipping[0]; _imu_down_sampled.delta_vel_clipping[1] |= imu_sample_new.delta_vel_clipping[1]; _imu_down_sampled.delta_vel_clipping[2] |= imu_sample_new.delta_vel_clipping[2]; + _imu_down_sampled.accel_reset |= imu_sample_new.accel_reset; + _imu_down_sampled.gyro_reset |= imu_sample_new.gyro_reset; // use a quaternion to accumulate delta angle data // this quaternion represents the rotation from the start to end of the accumulation period diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index d7d977f97c..7686965a54 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -448,12 +448,11 @@ void EKF2::Run() imu_sample_new.delta_ang = Vector3f{imu.delta_angle}; imu_sample_new.delta_vel_dt = imu.delta_velocity_dt * 1.e-6f; imu_sample_new.delta_vel = Vector3f{imu.delta_velocity}; - - if (imu.delta_velocity_clipping > 0) { - imu_sample_new.delta_vel_clipping[0] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_X; - imu_sample_new.delta_vel_clipping[1] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_Y; - imu_sample_new.delta_vel_clipping[2] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_Z; - } + imu_sample_new.delta_vel_clipping[0] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_X; + imu_sample_new.delta_vel_clipping[1] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_Y; + imu_sample_new.delta_vel_clipping[2] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_Z; + imu_sample_new.accel_reset = false; + imu_sample_new.gyro_reset = false; imu_dt = imu.delta_angle_dt; @@ -468,26 +467,20 @@ void EKF2::Run() || (imu.accel_device_id != _device_id_accel)) { PX4_DEBUG("%d - resetting accelerometer bias", _instance); + imu_sample_new.accel_reset = true; _device_id_accel = imu.accel_device_id; - - _ekf.resetAccelBias(); _accel_calibration_count = imu.accel_calibration_count; - - // reset bias learning - _accel_cal = {}; + _accel_cal = {}; // reset bias learning } if ((imu.gyro_calibration_count != _gyro_calibration_count) || (imu.gyro_device_id != _device_id_gyro)) { PX4_DEBUG("%d - resetting rate gyro bias", _instance); + imu_sample_new.gyro_reset = true; _device_id_gyro = imu.gyro_device_id; - - _ekf.resetGyroBias(); _gyro_calibration_count = imu.gyro_calibration_count; - - // reset bias learning - _gyro_cal = {}; + _gyro_cal = {}; // reset bias learning } } } @@ -507,35 +500,26 @@ void EKF2::Run() imu_sample_new.delta_ang = Vector3f{sensor_combined.gyro_rad} * imu_sample_new.delta_ang_dt; imu_sample_new.delta_vel_dt = sensor_combined.accelerometer_integral_dt * 1.e-6f; imu_sample_new.delta_vel = Vector3f{sensor_combined.accelerometer_m_s2} * imu_sample_new.delta_vel_dt; - - if (sensor_combined.accelerometer_clipping > 0) { - imu_sample_new.delta_vel_clipping[0] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_X; - imu_sample_new.delta_vel_clipping[1] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_Y; - imu_sample_new.delta_vel_clipping[2] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_Z; - } + imu_sample_new.delta_vel_clipping[0] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_X; + imu_sample_new.delta_vel_clipping[1] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_Y; + imu_sample_new.delta_vel_clipping[2] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_Z; + imu_sample_new.accel_reset = false; + imu_sample_new.gyro_reset = false; imu_dt = sensor_combined.gyro_integral_dt; if (sensor_combined.accel_calibration_count != _accel_calibration_count) { - PX4_DEBUG("%d - resetting accelerometer bias", _instance); - - _ekf.resetAccelBias(); + imu_sample_new.accel_reset = true; _accel_calibration_count = sensor_combined.accel_calibration_count; - - // reset bias learning - _accel_cal = {}; + _accel_cal = {}; // reset bias learning } if (sensor_combined.gyro_calibration_count != _gyro_calibration_count) { - PX4_DEBUG("%d - resetting rate gyro bias", _instance); - - _ekf.resetGyroBias(); + imu_sample_new.gyro_reset = true; _gyro_calibration_count = sensor_combined.gyro_calibration_count; - - // reset bias learning - _gyro_cal = {}; + _gyro_cal = {}; // reset bias learning } } @@ -544,23 +528,15 @@ void EKF2::Run() if (_sensor_selection_sub.copy(&sensor_selection)) { if (_device_id_accel != sensor_selection.accel_device_id) { - + imu_sample_new.accel_reset = true; _device_id_accel = sensor_selection.accel_device_id; - - _ekf.resetAccelBias(); - - // reset bias learning - _accel_cal = {}; + _accel_cal = {}; // reset bias learning } if (_device_id_gyro != sensor_selection.gyro_device_id) { - + imu_sample_new.gyro_reset = true; _device_id_gyro = sensor_selection.gyro_device_id; - - _ekf.resetGyroBias(); - - // reset bias learning - _gyro_cal = {}; + _gyro_cal = {}; // reset bias learning } } }