fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-03 19:00:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

save learned mag bias per sensor (Multi-EKF support)

Browse Source 
- handle saving the mag bias per sensor (across all estimator instances using that mag) in sensors/vehicle_magnetometer
 - this is now saving back to the actual mag calibration CAL_MAGn_OFF{X,Y,Z}
 - ekf2 reset mag mag bias on any magnetometer or calibration change
 - use Kalman filter scheme to update stored mag bias parameters using all available bias estimates for that sensor

Co-authored-by: Paul Riseborough <gncsolns@gmail.com>
This commit is contained in:
Daniel Agar
2021-01-07 09:54:13 -05:00
committed by GitHub
parent 537ee5b19b
commit 88f8da27ef
17 changed files with 328 additions and 308 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+47 -101
View File
@@ -202,24 +202,6 @@ int EKF2::print_status()
return 0;
}
template<typename Param>
void EKF2::update_mag_bias(Param &mag_bias_param, int axis_index)
{
// calculate weighting using ratio of variances and update stored bias values
const float weighting = constrain(_param_ekf2_magb_vref.get() / (_param_ekf2_magb_vref.get() +
_last_valid_variance(axis_index)), 0.0f, _param_ekf2_magb_k.get());
const float mag_bias_saved = mag_bias_param.get();
_last_valid_mag_cal(axis_index) = weighting * _last_valid_mag_cal(axis_index) + mag_bias_saved;
mag_bias_param.set(_last_valid_mag_cal(axis_index));
// save new parameters unless in multi-instance mode
if (!_multi_mode) {
mag_bias_param.commit_no_notification();
}
}
void EKF2::Run()
{
if (should_exit()) {
@@ -874,39 +856,39 @@ void EKF2::PublishSensorBias(const hrt_abstime &timestamp)
const Vector3f gyro_bias{_ekf.getGyroBias()};
const Vector3f accel_bias{_ekf.getAccelBias()};
// the saved mag bias EKF2_MAGBIAS_{X,Y,Z} is subtracted from sensor mag before going into ecl/EKF
const Vector3f mag_cal{_param_ekf2_magbias_x.get(), _param_ekf2_magbias_y.get(), _param_ekf2_magbias_z.get()};
const Vector3f mag_bias{_ekf.getMagBias() + mag_cal};
const Vector3f mag_bias{_mag_cal_last_bias};
// only publish on change
if ((gyro_bias - _last_gyro_bias).longerThan(0.001f)
|| (accel_bias - _last_accel_bias).longerThan(0.001f)
|| (mag_bias - _last_mag_bias).longerThan(0.001f)) {
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)) {
// take device ids from sensor_selection_s if not using specific vehicle_imu_s
if (_device_id_gyro != 0) {
bias.gyro_device_id = _device_id_gyro;
gyro_bias.copyTo(bias.gyro_bias);
_ekf.getGyroBiasVariance().copyTo(bias.gyro_bias_variance);
bias.gyro_bias_valid = true;
_last_gyro_bias = gyro_bias;
_last_gyro_bias_published = gyro_bias;
}
if ((_device_id_accel != 0) && !(_param_ekf2_aid_mask.get() & MASK_INHIBIT_ACC_BIAS)) {
bias.accel_device_id = _device_id_accel;
accel_bias.copyTo(bias.accel_bias);
_ekf.getAccelBiasVariance().copyTo(bias.accel_bias_variance);
bias.accel_bias_valid = !_ekf.fault_status_flags().bad_acc_bias;
_last_accel_bias = accel_bias;
_last_accel_bias_published = accel_bias;
}
if (_device_id_mag != 0) {
bias.mag_device_id = _device_id_mag;
mag_bias.copyTo(bias.mag_bias);
_ekf.getMagBiasVariance().copyTo(bias.mag_bias_variance);
_mag_cal_last_bias_variance.copyTo(bias.mag_bias_variance);
bias.mag_bias_valid = _mag_cal_available;
_last_mag_bias = mag_bias;
_last_mag_bias_published = mag_bias;
}
bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
@@ -1389,47 +1371,35 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
vehicle_magnetometer_s magnetometer;
if (_magnetometer_sub.update(&magnetometer)) {
// Reset learned bias parameters if there has been a persistant change in magnetometer ID
// Do not reset parmameters when armed to prevent potential time slips casued by parameter set
// and notification events
// Check if there has been a persistant change in magnetometer ID
if (magnetometer.device_id != 0
&& (magnetometer.device_id != (uint32_t)_param_ekf2_magbias_id.get())) {
if (_invalid_mag_id_count < 200) {
_invalid_mag_id_count++;
bool reset = false;
// check if magnetometer has changed
if (magnetometer.device_id != _device_id_mag) {
if (_device_id_mag != 0) {
PX4_WARN("%d - mag sensor ID changed %d -> %d", _instance, _device_id_mag, magnetometer.device_id);
}
} else {
if (_invalid_mag_id_count > 0) {
_invalid_mag_id_count--;
}
reset = true;
} else if (magnetometer.calibration_count > _mag_calibration_count) {
// existing calibration has changed, reset saved mag bias
PX4_DEBUG("%d - mag %d calibration updated, resetting bias", _instance, _device_id_mag);
reset = true;
}
_device_id_mag = magnetometer.device_id;
if (reset) {
_ekf.resetMagBias();
_device_id_mag = magnetometer.device_id;
_mag_calibration_count = magnetometer.calibration_count;
if (!_armed && (_invalid_mag_id_count > 100)) {
// the sensor ID used for the last saved mag bias is not confirmed to be the same as the current sensor ID
// this means we need to reset the learned bias values to zero
_param_ekf2_magbias_x.set(0.f);
_param_ekf2_magbias_y.set(0.f);
_param_ekf2_magbias_z.set(0.f);
_param_ekf2_magbias_id.set(magnetometer.device_id);
if (!_multi_mode) {
_param_ekf2_magbias_x.reset();
_param_ekf2_magbias_y.reset();
_param_ekf2_magbias_z.reset();
_param_ekf2_magbias_id.commit();
PX4_INFO("%d - mag sensor ID changed to %i", _instance, _param_ekf2_magbias_id.get());
}
_invalid_mag_id_count = 0;
// reset magnetometer bias learning
_mag_cal_total_time_us = 0;
_mag_cal_last_us = 0;
_mag_cal_available = false;
}
const Vector3f mag_cal_bias{_param_ekf2_magbias_x.get(), _param_ekf2_magbias_y.get(), _param_ekf2_magbias_z.get()};
_ekf.setMagData(magSample{magnetometer.timestamp_sample, Vector3f{magnetometer.magnetometer_ga} - mag_cal_bias});
_ekf.setMagData(magSample{magnetometer.timestamp_sample, Vector3f{magnetometer.magnetometer_ga}});
ekf2_timestamps.vehicle_magnetometer_timestamp_rel = (int16_t)((int64_t)magnetometer.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
@@ -1484,56 +1454,32 @@ void EKF2::UpdateMagCalibration(const hrt_abstime &timestamp)
// the EKF is operating in the correct mode and there are no filter faults
if (_ekf.control_status_flags().in_air && _ekf.control_status_flags().mag_3D && (_ekf.fault_status().value == 0)) {
if (_last_magcal_us == 0) {
_last_magcal_us = timestamp;
if (_mag_cal_last_us != 0) {
_mag_cal_total_time_us += timestamp - _mag_cal_last_us;
} else {
_total_cal_time_us += timestamp - _last_magcal_us;
_last_magcal_us = timestamp;
}
// Start checking mag bias estimates when we have accumulated sufficient calibration time
if (_total_cal_time_us > 30_s) {
// we have sufficient accumulated valid flight time to form a reliable bias estimate
// check that the state variance for each axis is within a range indicating filter convergence
const float max_var_allowed = 100.0f * _param_ekf2_magb_vref.get();
const float min_var_allowed = 0.01f * _param_ekf2_magb_vref.get();
// Declare all bias estimates invalid if any variances are out of range
const Vector3f mag_bias_variance{_ekf.getMagBiasVariance()};
// Store valid estimates and their associated variances
if ((mag_bias_variance.min() >= min_var_allowed) && (mag_bias_variance.max() <= max_var_allowed)) {
_last_valid_mag_cal = _ekf.getMagBias();
_last_valid_variance = mag_bias_variance;
_valid_cal_available = true;
// 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_available = true;
}
}
} else 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.
_total_cal_time_us = 0;
_valid_cal_available = false;
_mag_cal_last_us = timestamp;
} else {
// conditions are NOT OK for learning magnetometer bias, reset timestamp
// but keep the accumulated calibration time
_last_magcal_us = timestamp;
_mag_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.
_mag_cal_total_time_us = 0;
}
}
// Check and save the last valid calibration when we are disarmed
if (!_armed) {
if (_valid_cal_available) {
update_mag_bias(_param_ekf2_magbias_x, 0);
update_mag_bias(_param_ekf2_magbias_y, 1);
update_mag_bias(_param_ekf2_magbias_z, 2);
// reset to prevent data being saved too frequently
_total_cal_time_us = 0;
_valid_cal_available = false;
}
// update stored declination value
if (!_mag_decl_saved) {
float declination_deg;