Skip to content

Commit

Permalink
ekf2: move predict covariance IMU inhibit check to function
Browse files Browse the repository at this point in the history
  • Loading branch information
@dagar
dagar committed Oct 17, 2023
1 parent 0b44852 commit 48e09a4
Show file tree
Hide file tree
Showing 4 changed files with 105 additions and 90 deletions.
87 changes: 4 additions & 83 deletions src/modules/ekf2/EKF/covariance.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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++) {
Expand Down
15 changes: 8 additions & 7 deletions src/modules/ekf2/EKF/ekf.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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
Expand Down
2 changes: 2 additions & 0 deletions src/modules/ekf2/EKF/ekf.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -822,6 +822,8 @@ class Ekf final : public EstimatorInterface
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);
Expand Down
91 changes: 91 additions & 0 deletions src/modules/ekf2/EKF/ekf_helper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
}
}

}

0 comments on commit 48e09a4

Please sign in to comment.