ekf2: extend sensor bias stability requirement

- for accel/gyro/mag estimated bias only consider them stable (valid
for calibration updates) if the value isn't changing (10% of limit) over
the validity period
This commit is contained in:
Daniel Agar
2022-09-28 10:40:52 -04:00
parent 16f8adb4b3
commit 1f7080a710
4 changed files with 84 additions and 100 deletions
+10 -7
View File
@@ -171,6 +171,16 @@ private:
void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps);
void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps);
// Used to check, save and use learned accel/gyro/mag biases
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
matrix::Vector3f bias{};
bool cal_available{false}; ///< true when an unsaved valid calibration for the XYZ accelerometer bias is available
};
void UpdateCalibration(const hrt_abstime &timestamp, InFlightCalibration &cal, const matrix::Vector3f &bias,
const matrix::Vector3f &bias_variance, float bias_limit, bool bias_valid, bool learning_valid);
void UpdateAccelCalibration(const hrt_abstime &timestamp);
void UpdateGyroCalibration(const hrt_abstime &timestamp);
void UpdateMagCalibration(const hrt_abstime &timestamp);
@@ -225,13 +235,6 @@ private:
// Used to control saving of mag declination to be used on next startup
bool _mag_decl_saved = false; ///< true when the magnetic declination has been saved
// Used to check, save and use learned accel/gyro/mag biases
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
bool cal_available{false}; ///< true when an unsaved valid calibration for the XYZ accelerometer bias is available
};
InFlightCalibration _accel_cal{};
InFlightCalibration _gyro_cal{};
InFlightCalibration _mag_cal{};