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

ekf2: handle IMU calibration changes on delayed time horizon

Browse Source 
- reset biases and covariances on delayed time horizon in addition to
resetting immediately (for the sake of the output predictor)
This commit is contained in:
Daniel Agar 2022-11-02 17:20:40 -04:00
parent 21c7f8ad74
commit 0ca0e954fb
No known key found for this signature in database
GPG Key ID: FD3CBA98017A69DE
11 changed files with 66 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/SensorCombined.msg
View File

@ -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.

2
msg/VehicleImu.msg
View File

@ -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.

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

@ -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 {

15
src/modules/ekf2/EKF/covariance.cpp
View File

@ -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();

12
src/modules/ekf2/EKF/ekf.cpp
View File

@ -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);

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

@ -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(); };

13
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -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:

12
src/modules/ekf2/EKF/estimator_interface.cpp
View File

@ -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);

3
src/modules/ekf2/EKF/estimator_interface.h
View File

@ -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; }

2
src/modules/ekf2/EKF/imu_down_sampler.cpp
View File

@ -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

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

@ -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
}
}
}