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

ekf2: move predict covariance IMU inhibit check to function

This commit is contained in:
Daniel Agar 2023-10-09 13:54:25 -04:00
parent 0b44852094
commit 48e09a4dea
4 changed files with 105 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -102,91 +102,12 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
// Use average update interval to reduce accumulated covariance prediction errors due to small single frame dt values
const float dt = _dt_ekf_avg;
// inhibit learning of imu accel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities or bad accel data is detected
// xy accel bias learning is also disabled on ground as those states are poorly observable when perpendicular to the gravity vector
const float alpha = math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f);
const float beta = 1.0f - alpha;
_ang_rate_magnitude_filt = fmaxf(imu_delayed.delta_ang.norm() / imu_delayed.delta_ang_dt, beta * _ang_rate_magnitude_filt);
_accel_magnitude_filt = fmaxf(imu_delayed.delta_vel.norm() / imu_delayed.delta_vel_dt, beta * _accel_magnitude_filt);
_accel_vec_filt = alpha * imu_delayed.delta_vel / imu_delayed.delta_vel_dt + beta * _accel_vec_filt;
const bool is_manoeuvre_level_high = _ang_rate_magnitude_filt > _params.acc_bias_learn_gyr_lim
|| _accel_magnitude_filt > _params.acc_bias_learn_acc_lim;
// gyro bias inhibit
const bool do_inhibit_all_gyro_axes = !(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GyroBias));
for (unsigned index = 0; index < State::gyro_bias.dof; index++) {
const unsigned stateIndex = State::gyro_bias.idx + index;
bool is_bias_observable = true;
// TODO: gyro bias conditions
const bool do_inhibit_axis = do_inhibit_all_gyro_axes || !is_bias_observable;
if (do_inhibit_axis) {
// store the bias state variances to be reinstated later
if (!_gyro_bias_inhibit[index]) {
_prev_gyro_bias_var(index) = P(stateIndex, stateIndex);
_gyro_bias_inhibit[index] = true;
}
} else {
if (_gyro_bias_inhibit[index]) {
// reinstate the bias state variances
P(stateIndex, stateIndex) = _prev_gyro_bias_var(index);
_gyro_bias_inhibit[index] = false;
}
}
}
// accel bias inhibit
const bool do_inhibit_all_accel_axes = !(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::AccelBias))
|| is_manoeuvre_level_high
|| _fault_status.flags.bad_acc_vertical;
for (unsigned index = 0; index < State::accel_bias.dof; index++) {
const unsigned stateIndex = State::accel_bias.idx + index;
bool is_bias_observable = true;
if (_control_status.flags.vehicle_at_rest) {
is_bias_observable = true;
} else if (_control_status.flags.fake_hgt) {
is_bias_observable = false;
} else if (_control_status.flags.fake_pos) {
// when using fake position (but not fake height) only consider an accel bias observable if aligned with the gravity vector
is_bias_observable = (fabsf(_R_to_earth(2, index)) > 0.966f); // cos 15 degrees ~= 0.966
}
const bool do_inhibit_axis = do_inhibit_all_accel_axes || imu_delayed.delta_vel_clipping[index] || !is_bias_observable;
if (do_inhibit_axis) {
// store the bias state variances to be reinstated later
if (!_accel_bias_inhibit[index]) {
_prev_accel_bias_var(index) = P(stateIndex, stateIndex);
_accel_bias_inhibit[index] = true;
}
} else {
if (_accel_bias_inhibit[index]) {
// reinstate the bias state variances
P(stateIndex, stateIndex) = _prev_accel_bias_var(index);
_accel_bias_inhibit[index] = false;
}
}
}
// assign IMU noise variances
// inputs to the system are 3 delta angles and 3 delta velocities
float gyro_noise = math::constrain(_params.gyro_noise, 0.0f, 1.0f);
// delta angle noise variance
float gyro_noise = math::constrain(_params.gyro_noise, 0.f, 1.f);
const float d_ang_var = sq(imu_delayed.delta_ang_dt * gyro_noise);
float accel_noise = math::constrain(_params.accel_noise, 0.0f, 1.0f);
// delta velocity noise variance
float accel_noise = math::constrain(_params.accel_noise, 0.f, 1.f);
Vector3f d_vel_var;
for (unsigned i = 0; i < 3; i++) {

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

@ -195,6 +195,14 @@ bool Ekf::update()
// TODO: explicitly pop at desired time horizon
const imuSample imu_sample_delayed = _imu_buffer.get_oldest();
// calculate an average filter update time
// filter and limit input between -50% and +100% of nominal value
float input = 0.5f * (imu_sample_delayed.delta_vel_dt + imu_sample_delayed.delta_ang_dt);
float filter_update_s = 1e-6f * _params.filter_update_interval_us;
_dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * math::constrain(input, 0.5f * filter_update_s, 2.f * filter_update_s);
updateIMUBiasInhibit(imu_sample_delayed);
// perform state and covariance prediction for the main filter
predictCovariance(imu_sample_delayed);
predictState(imu_sample_delayed);
@ -305,13 +313,6 @@ void Ekf::predictState(const imuSample &imu_delayed)
constrainStates();
// calculate an average filter update time
float input = 0.5f * (imu_delayed.delta_vel_dt + imu_delayed.delta_ang_dt);
// filter and limit input between -50% and +100% of nominal value
const float filter_update_s = 1e-6f * _params.filter_update_interval_us;
input = math::constrain(input, 0.5f * filter_update_s, 2.f * filter_update_s);
_dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * input;
// some calculations elsewhere in code require a raw angular rate vector so calculate here to avoid duplication
// protect against possible small timesteps resulting from timing slip on previous frame that can drive spikes into the rate

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

@ -822,6 +822,8 @@ private:
bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW);
void computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const;
void updateIMUBiasInhibit(const imuSample &imu_delayed);
#if defined(CONFIG_EKF2_MAGNETOMETER)
// ekf sequential fusion of magnetometer measurements
bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true);

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

@ -1107,3 +1107,94 @@ void Ekf::resetWindCov()
P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, sq(_params.initial_wind_uncertainty));
}
#endif // CONFIG_EKF2_WIND
void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
{
// inhibit learning of imu accel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities or bad accel data is detected
// xy accel bias learning is also disabled on ground as those states are poorly observable when perpendicular to the gravity vector
{
const float alpha = math::constrain((imu_delayed.delta_ang_dt / _params.acc_bias_learn_tc), 0.f, 1.f);
const float beta = 1.f - alpha;
_ang_rate_magnitude_filt = fmaxf(imu_delayed.delta_ang.norm() / imu_delayed.delta_ang_dt, beta * _ang_rate_magnitude_filt);
}
{
const float alpha = math::constrain((imu_delayed.delta_vel_dt / _params.acc_bias_learn_tc), 0.f, 1.f);
const float beta = 1.f - alpha;
_accel_magnitude_filt = fmaxf(imu_delayed.delta_vel.norm() / imu_delayed.delta_vel_dt, beta * _accel_magnitude_filt);
_accel_vec_filt = alpha * imu_delayed.delta_vel / imu_delayed.delta_vel_dt + beta * _accel_vec_filt;
}
const bool is_manoeuvre_level_high = (_ang_rate_magnitude_filt > _params.acc_bias_learn_gyr_lim)
|| (_accel_magnitude_filt > _params.acc_bias_learn_acc_lim);
// gyro bias inhibit
const bool do_inhibit_all_gyro_axes = !(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GyroBias));
for (unsigned index = 0; index < State::gyro_bias.dof; index++) {
const unsigned stateIndex = State::gyro_bias.idx + index;
bool is_bias_observable = true;
// TODO: gyro bias conditions
const bool do_inhibit_axis = do_inhibit_all_gyro_axes || !is_bias_observable;
if (do_inhibit_axis) {
// store the bias state variances to be reinstated later
if (!_gyro_bias_inhibit[index]) {
_prev_gyro_bias_var(index) = P(stateIndex, stateIndex);
_gyro_bias_inhibit[index] = true;
}
} else {
if (_gyro_bias_inhibit[index]) {
// reinstate the bias state variances
P(stateIndex, stateIndex) = _prev_gyro_bias_var(index);
_gyro_bias_inhibit[index] = false;
}
}
}
// accel bias inhibit
const bool do_inhibit_all_accel_axes = !(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::AccelBias))
|| is_manoeuvre_level_high
|| _fault_status.flags.bad_acc_vertical;
for (unsigned index = 0; index < State::accel_bias.dof; index++) {
const unsigned stateIndex = State::accel_bias.idx + index;
bool is_bias_observable = true;
if (_control_status.flags.vehicle_at_rest) {
is_bias_observable = true;
} else if (_control_status.flags.fake_hgt) {
is_bias_observable = false;
} else if (_control_status.flags.fake_pos) {
// when using fake position (but not fake height) only consider an accel bias observable if aligned with the gravity vector
is_bias_observable = (fabsf(_R_to_earth(2, index)) > 0.966f); // cos 15 degrees ~= 0.966
}
const bool do_inhibit_axis = do_inhibit_all_accel_axes || imu_delayed.delta_vel_clipping[index] || !is_bias_observable;
if (do_inhibit_axis) {
// store the bias state variances to be reinstated later
if (!_accel_bias_inhibit[index]) {
_prev_accel_bias_var(index) = P(stateIndex, stateIndex);
_accel_bias_inhibit[index] = true;
}
} else {
if (_accel_bias_inhibit[index]) {
// reinstate the bias state variances
P(stateIndex, stateIndex) = _prev_accel_bias_var(index);
_accel_bias_inhibit[index] = false;
}
}
}
}