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

ekf2: expand accel bias stability criteria

This commit is contained in:
Daniel Agar 2021-11-17 15:02:35 -05:00
parent 846f807eff
commit 170849c8f8
5 changed files with 73 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/ekf2/EKF/common.h
View File

@ -548,7 +548,7 @@ union warning_event_status_u {
bool gps_data_stopped_using_alternate : 1; ///< 3 - true when the gps data has stopped for a significant time period but the filter is able to use other sources of data to maintain navigation
bool height_sensor_timeout : 1; ///< 4 - true when the height sensor has not been used to correct the state estimates for a significant time period
bool stopping_navigation : 1; ///< 5 - true when the filter has insufficient data to estimate velocity and position and is falling back to an attitude, height and height rate mode of operation
bool invalid_accel_bias_cov_reset : 1; ///< 6 - true when the filter has detected bad acceerometer bias state esitmstes and has reset the corresponding covariance matrix elements
bool invalid_accel_bias_cov_reset : 1; ///< 6 - true when the filter has detected bad acceerometer bias state estimates and has reset the corresponding covariance matrix elements
bool bad_yaw_using_gps_course : 1; ///< 7 - true when the fiter has detected an invalid yaw esitmate and has reset the yaw angle to the GPS ground course
bool stopping_mag_use : 1; ///< 8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetomer data
bool vision_data_stopped : 1; ///< 9 - true when the vision system data has stopped for a significant time period

2
src/modules/ekf2/EKF/ekf.h
View File

@ -244,6 +244,8 @@ public:
Vector3f getAccelBiasVariance() const { return Vector3f{P(13, 13), P(14, 14), P(15, 15)} / sq(_dt_ekf_avg); } // get the accelerometer bias variance in m/s**2
Vector3f getMagBiasVariance() const { return Vector3f{P(19, 19), P(20, 20), P(21, 21)}; }
bool accel_bias_inhibited() const { return _accel_bias_inhibit[0] || _accel_bias_inhibit[1] || _accel_bias_inhibit[2]; }
// get GPS check status
void get_gps_check_status(uint16_t *val) const { *val = _gps_check_fail_status.value; }

89
src/modules/ekf2/EKF2.cpp
View File

@ -1057,17 +1057,18 @@ void EKF2::PublishOdometryAligned(const hrt_abstime &timestamp, const vehicle_od
void EKF2::PublishSensorBias(const hrt_abstime &timestamp)
{
// estimator_sensor_bias
estimator_sensor_bias_s bias{};
bias.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
const Vector3f gyro_bias{_ekf.getGyroBias()};
const Vector3f accel_bias{_ekf.getAccelBias()};
const Vector3f mag_bias{_ekf.getMagBias()};
// only publish on change
// publish at ~1 Hz, or sooner if there's a change
if ((gyro_bias - _last_gyro_bias_published).longerThan(0.001f)
|| (accel_bias - _last_accel_bias_published).longerThan(0.001f)
|| (mag_bias - _last_mag_bias_published).longerThan(0.001f)) {
|| (mag_bias - _last_mag_bias_published).longerThan(0.001f)
|| (timestamp >= _last_sensor_bias_published + 1_s)) {
estimator_sensor_bias_s bias{};
bias.timestamp_sample = timestamp;
// take device ids from sensor_selection_s if not using specific vehicle_imu_s
if (_device_id_gyro != 0) {
@ -1102,6 +1103,8 @@ void EKF2::PublishSensorBias(const hrt_abstime &timestamp)
bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_sensor_bias_pub.publish(bias);
_last_sensor_bias_published = bias.timestamp;
}
}
@ -1779,18 +1782,40 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
void EKF2::UpdateAccelCalibration(const hrt_abstime &timestamp)
{
if (_param_ekf2_aid_mask.get() & MASK_INHIBIT_ACC_BIAS) {
_accel_cal.cal_available = false;
return;
}
// State variance assumed for accelerometer bias storage.
// This is a reference variance used to calculate the fraction of learned accelerometer bias that will be used to update the stored value.
// Larger values cause a larger fraction of the learned biases to be used.
static constexpr float max_var_allowed = 1e-3f;
static constexpr float max_var_ratio = 1e2f;
const Vector3f bias_variance{_ekf.getAccelBiasVariance()};
// Check if conditions are OK for learning of accelerometer bias values
// the EKF is operating in the correct mode and there are no filter faults
if (_ekf.control_status_flags().in_air && (_ekf.fault_status().value == 0)
&& !(_param_ekf2_aid_mask.get() & MASK_INHIBIT_ACC_BIAS)) {
if ((_ekf.fault_status().value == 0)
&& !_ekf.accel_bias_inhibited()
&& !_preflt_checker.hasHorizFailed() && !_preflt_checker.hasVertFailed()
&& (_ekf.control_status_flags().baro_hgt || _ekf.control_status_flags().rng_hgt
|| _ekf.control_status_flags().gps_hgt || _ekf.control_status_flags().ev_hgt)
&& !_ekf.warning_event_flags().height_sensor_timeout && !_ekf.warning_event_flags().invalid_accel_bias_cov_reset
&& !_ekf.innov_check_fail_status_flags().reject_ver_pos && !_ekf.innov_check_fail_status_flags().reject_ver_vel
&& (bias_variance.max() < max_var_allowed) && (bias_variance.max() < max_var_ratio * bias_variance.min())
) {
if (_accel_cal.last_us != 0) {
_accel_cal.total_time_us += timestamp - _accel_cal.last_us;
// Start checking accel bias estimates when we have accumulated sufficient calibration time
if (_accel_cal.total_time_us > 30_s) {
_accel_cal.last_bias = _ekf.getAccelBias();
_accel_cal.last_bias_variance = _ekf.getAccelBiasVariance();
if (!_accel_cal.cal_available) {
PX4_DEBUG("%d accel bias now stable", _instance);
}
_accel_cal.cal_available = true;
}
}
@ -1798,31 +1823,40 @@ void EKF2::UpdateAccelCalibration(const hrt_abstime &timestamp)
_accel_cal.last_us = timestamp;
} else {
// conditions are NOT OK for learning accelerometer bias, reset timestamp
// but keep the accumulated calibration time
_accel_cal.last_us = 0;
if (_ekf.fault_status().value != 0) {
// if a filter fault has occurred, assume previous learning was invalid and do not
// count it towards total learning time.
_accel_cal.total_time_us = 0;
// conditions are NOT OK for learning accelerometer bias, reset
if (_accel_cal.total_time_us > 0) {
PX4_DEBUG("%d, clearing learned accel bias", _instance);
}
_accel_cal = {};
}
}
void EKF2::UpdateGyroCalibration(const hrt_abstime &timestamp)
{
// State variance assumed for accelerometer bias storage.
// This is a reference variance used to calculate the fraction of learned accelerometer bias that will be used to update the stored value.
// Larger values cause a larger fraction of the learned biases to be used.
static constexpr float max_var_allowed = 1e-3f;
static constexpr float max_var_ratio = 1e2f;
const Vector3f bias_variance{_ekf.getGyroBiasVariance()};
// Check if conditions are OK for learning of gyro bias values
// the EKF is operating in the correct mode and there are no filter faults
if (_ekf.control_status_flags().in_air && (_ekf.fault_status().value == 0)) {
if ((_ekf.fault_status().value == 0)
&& (bias_variance.max() < max_var_allowed) && (bias_variance.max() < max_var_ratio * bias_variance.min())
) {
if (_gyro_cal.last_us != 0) {
_gyro_cal.total_time_us += timestamp - _gyro_cal.last_us;
// Start checking gyro bias estimates when we have accumulated sufficient calibration time
if (_gyro_cal.total_time_us > 30_s) {
_gyro_cal.last_bias = _ekf.getGyroBias();
_gyro_cal.last_bias_variance = _ekf.getGyroBiasVariance();
if (!_gyro_cal.cal_available) {
PX4_DEBUG("%d gyro bias now stable", _instance);
}
_gyro_cal.cal_available = true;
}
}
@ -1830,15 +1864,12 @@ void EKF2::UpdateGyroCalibration(const hrt_abstime &timestamp)
_gyro_cal.last_us = timestamp;
} else {
// conditions are NOT OK for learning gyro bias, reset timestamp
// but keep the accumulated calibration time
_gyro_cal.last_us = 0;
if (_ekf.fault_status().value != 0) {
// if a filter fault has occurred, assume previous learning was invalid and do not
// count it towards total learning time.
_gyro_cal.total_time_us = 0;
// conditions are NOT OK for learning gyro bias, reset
if (_gyro_cal.total_time_us > 0) {
PX4_DEBUG("%d, clearing learned gyro bias", _instance);
}
_gyro_cal = {};
}
}
@ -1853,8 +1884,6 @@ void EKF2::UpdateMagCalibration(const hrt_abstime &timestamp)
// Start checking mag bias estimates when we have accumulated sufficient calibration time
if (_mag_cal.total_time_us > 30_s) {
_mag_cal.last_bias = _ekf.getMagBias();
_mag_cal.last_bias_variance = _ekf.getMagBiasVariance();
_mag_cal.cal_available = true;
}
}

4
src/modules/ekf2/EKF2.hpp
View File

@ -208,8 +208,6 @@ private:
struct InFlightCalibration {
hrt_abstime last_us{0}; ///< last time the EKF was operating a mode that estimates accelerometer biases (uSec)
hrt_abstime total_time_us{0}; ///< accumulated calibration time since the last save
Vector3f last_bias{}; ///< last valid XYZ accelerometer bias estimates (Gauss)
Vector3f last_bias_variance{}; ///< variances for the last valid accelerometer XYZ bias estimates (m/s**2)**2
bool cal_available{false}; ///< true when an unsaved valid calibration for the XYZ accelerometer bias is available
};
@ -241,6 +239,8 @@ private:
Vector3f _last_gyro_calibration_published{};
Vector3f _last_mag_calibration_published{};
hrt_abstime _last_sensor_bias_published{0};
float _last_baro_bias_published{};
float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements

28
src/modules/sensors/vehicle_imu/VehicleIMU.cpp
View File

@ -683,27 +683,20 @@ void VehicleIMU::PrintStatus()
void VehicleIMU::SensorCalibrationUpdate()
{
// State variance assumed for accelerometer bias storage.
// This is a reference variance used to calculate the fraction of learned accelerometer bias that will be used to update the stored value.
// Larger values cause a larger fraction of the learned biases to be used.
static constexpr float max_var_allowed = 1e-3f;
static constexpr float max_var_ratio = 1e2f;
if (_armed) {
for (int i = 0; i < _estimator_sensor_bias_subs.size(); i++) {
estimator_sensor_bias_s estimator_sensor_bias;
if (_estimator_sensor_bias_subs[i].update(&estimator_sensor_bias)) {
// find corresponding accel bias
if (_accel_calibration.device_id() == estimator_sensor_bias.accel_device_id) {
if ((estimator_sensor_bias.accel_device_id != 0)
&& (_accel_calibration.device_id() == estimator_sensor_bias.accel_device_id)) {
const Vector3f bias{estimator_sensor_bias.accel_bias};
const Vector3f bias_variance{estimator_sensor_bias.accel_bias_variance};
const bool valid = (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s) &&
(estimator_sensor_bias.accel_device_id != 0) &&
estimator_sensor_bias.accel_bias_stable &&
(bias_variance.max() < max_var_allowed) &&
(bias_variance.max() < max_var_ratio * bias_variance.min());
const bool valid = (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s) && estimator_sensor_bias.accel_bias_valid
&& estimator_sensor_bias.accel_bias_stable;
if (valid) {
const Vector3f offset_old{_accel_learned_calibration[i].offset};
@ -728,16 +721,13 @@ void VehicleIMU::SensorCalibrationUpdate()
}
// find corresponding gyro calibration
if (_gyro_calibration.device_id() == estimator_sensor_bias.gyro_device_id) {
if ((estimator_sensor_bias.gyro_device_id != 0)
&& (_gyro_calibration.device_id() == estimator_sensor_bias.gyro_device_id)) {
const Vector3f bias{estimator_sensor_bias.gyro_bias};
const Vector3f bias_variance{estimator_sensor_bias.gyro_bias_variance};
const bool valid = (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s) &&
(estimator_sensor_bias.gyro_device_id != 0) &&
estimator_sensor_bias.gyro_bias_valid &&
estimator_sensor_bias.gyro_bias_stable &&
(bias_variance.max() < max_var_allowed) &&
(bias_variance.max() < max_var_ratio * bias_variance.min());
const bool valid = (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s) && estimator_sensor_bias.gyro_bias_valid
&& estimator_sensor_bias.gyro_bias_stable;
if (valid) {
const Vector3f offset_old{_gyro_learned_calibration[i].offset};