Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

ekf2: move accel bias check out of fixCovarianceErrors #22201

Merged
merged 2 commits into from
Oct 17, 2023
Merged

ekf2: move accel bias check out of fixCovarianceErrors #22201

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
ac1d2ac
ekf2: move accel bias check out of fixCovarianceErrors
dagar Oct 9, 2023
e54df4b
ekf2: move predict covariance IMU inhibit check to function
dagar Oct 9, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
192 changes: 15 additions & 177 deletions src/modules/ekf2/EKF/covariance.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,23 +84,16 @@ void Ekf::initialiseCovariance()

P.uncorrelateCovarianceSetVariance<State::pos.dof>(State::pos.idx, Vector3f(xy_pos_var, xy_pos_var, z_pos_var));

// gyro bias
const float gyro_bias_var = sq(_params.switch_on_gyro_bias);
P.uncorrelateCovarianceSetVariance<State::gyro_bias.dof>(State::gyro_bias.idx, gyro_bias_var);
_prev_gyro_bias_var.setAll(gyro_bias_var);
resetGyroBiasCov();

// accel bias
const float accel_bias_var = sq(_params.switch_on_accel_bias);
P.uncorrelateCovarianceSetVariance<State::accel_bias.dof>(State::accel_bias.idx, accel_bias_var);
_prev_accel_bias_var.setAll(accel_bias_var);
resetAccelBiasCov();

#if defined(CONFIG_EKF2_MAGNETOMETER)
resetMagCov();
#endif // CONFIG_EKF2_MAGNETOMETER

#if defined(CONFIG_EKF2_WIND)
// wind
P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, sq(_params.initial_wind_uncertainty));
resetWindCov();
#endif // CONFIG_EKF2_WIND
}

Expand All @@ -109,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 Expand Up @@ -317,14 +231,6 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
constrainStateVar(State::pos, 1e-6f, pos_var_max);
constrainStateVar(State::gyro_bias, 0.f, gyro_bias_var_max);

// force symmetry on the quaternion, velocity and position state covariances
if (force_symmetry) {
P.makeRowColSymmetric<State::quat_nominal.dof>(State::quat_nominal.idx);
P.makeRowColSymmetric<State::vel.dof>(State::vel.idx);
P.makeRowColSymmetric<State::pos.dof>(State::pos.idx);
P.makeRowColSymmetric<State::gyro_bias.dof>(State::gyro_bias.idx); //TODO: needed?
}

// the following states are optional and are deactivated when not required
// by ensuring the corresponding covariance matrix values are kept at zero

Expand Down Expand Up @@ -370,84 +276,16 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)

// If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero
if (resetRequired) {
P.uncorrelateCovariance<State::accel_bias.dof>(State::accel_bias.idx);
}

// Run additional checks to see if the delta velocity bias has hit limits in a direction that is clearly wrong
// calculate accel bias term aligned with the gravity vector
const float dVel_bias_lim = 0.9f * _params.acc_bias_lim * _dt_ekf_avg;
const Vector3f delta_vel_bias = _state.accel_bias * _dt_ekf_avg;
const float down_dvel_bias = delta_vel_bias.dot(Vector3f(_R_to_earth.row(2)));

// check that the vertical component of accel bias is consistent with both the vertical position and velocity innovation
bool bad_acc_bias = false;
if (fabsf(down_dvel_bias) > dVel_bias_lim) {
#if defined(CONFIG_EKF2_GNSS)
bool bad_vz_gps = _control_status.flags.gps && (down_dvel_bias * _aid_src_gnss_vel.innovation[2] < 0.0f);
#else
bool bad_vz_gps = false;
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
bool bad_vz_ev = _control_status.flags.ev_vel && (down_dvel_bias * _aid_src_ev_vel.innovation[2] < 0.0f);
#else
bool bad_vz_ev = false;
#endif // CONFIG_EKF2_EXTERNAL_VISION

if (bad_vz_gps || bad_vz_ev) {
#if defined(CONFIG_EKF2_BAROMETER)
bool bad_z_baro = _control_status.flags.baro_hgt && (down_dvel_bias * _aid_src_baro_hgt.innovation < 0.0f);
#else
bool bad_z_baro = false;
#endif
#if defined(CONFIG_EKF2_GNSS)
bool bad_z_gps = _control_status.flags.gps_hgt && (down_dvel_bias * _aid_src_gnss_hgt.innovation < 0.0f);
#else
bool bad_z_gps = false;
#endif

#if defined(CONFIG_EKF2_RANGE_FINDER)
bool bad_z_rng = _control_status.flags.rng_hgt && (down_dvel_bias * _aid_src_rng_hgt.innovation < 0.0f);
#else
bool bad_z_rng = false;
#endif // CONFIG_EKF2_RANGE_FINDER

#if defined(CONFIG_EKF2_EXTERNAL_VISION)
bool bad_z_ev = _control_status.flags.ev_hgt && (down_dvel_bias * _aid_src_ev_hgt.innovation < 0.0f);
#else
bool bad_z_ev = false;
#endif // CONFIG_EKF2_EXTERNAL_VISION

if (bad_z_baro || bad_z_gps || bad_z_rng || bad_z_ev) {
bad_acc_bias = true;
}
}
}

// record the pass/fail
if (!bad_acc_bias) {
_fault_status.flags.bad_acc_bias = false;
_time_acc_bias_check = _time_delayed_us;

} else {
_fault_status.flags.bad_acc_bias = true;
}

// if we have failed for 7 seconds continuously, reset the accel bias covariances to fix bad conditioning of
// the covariance matrix but preserve the variances (diagonals) to allow bias learning to continue
if (isTimedOut(_time_acc_bias_check, (uint64_t)7e6)) {

P.uncorrelateCovariance<State::accel_bias.dof>(State::accel_bias.idx);

_time_acc_bias_check = _time_delayed_us;
_fault_status.flags.bad_acc_bias = false;
_warning_events.flags.invalid_accel_bias_cov_reset = true;
ECL_WARN("invalid accel bias - covariance reset");

} else if (force_symmetry) {
// ensure the covariance values are symmetrical
P.makeRowColSymmetric<State::accel_bias.dof>(State::accel_bias.idx);
resetAccelBiasCov();
}
}

if (force_symmetry) {
P.makeRowColSymmetric<State::quat_nominal.dof>(State::quat_nominal.idx);
P.makeRowColSymmetric<State::vel.dof>(State::vel.idx);
P.makeRowColSymmetric<State::pos.dof>(State::pos.idx);
P.makeRowColSymmetric<State::gyro_bias.dof>(State::gyro_bias.idx);
P.makeRowColSymmetric<State::accel_bias.dof>(State::accel_bias.idx);
}

#if defined(CONFIG_EKF2_MAGNETOMETER)
Expand Down
15 changes: 8 additions & 7 deletions src/modules/ekf2/EKF/ekf.cpp
View file
Open in desktop
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
8 changes: 8 additions & 0 deletions src/modules/ekf2/EKF/ekf.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -358,8 +358,12 @@ class Ekf final : public EstimatorInterface

// Reset all IMU bias states and covariances to initial alignment values.
void resetImuBias();

void resetGyroBias();
void resetGyroBiasCov();

void resetAccelBias();
void resetAccelBiasCov();

// return true if the global position estimate is valid
// return true if the origin is set we are not doing unconstrained free inertial navigation
Expand Down Expand Up @@ -818,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 Expand Up @@ -1163,6 +1169,7 @@ class Ekf final : public EstimatorInterface
void stopAuxVelFusion();
#endif // CONFIG_EKF2_AUXVEL

void checkVerticalAccelerationBias(const imuSample &imu_delayed);
void checkVerticalAccelerationHealth(const imuSample &imu_delayed);
Likelihood estimateInertialNavFallingLikelihood() const;

Expand Down Expand Up @@ -1190,6 +1197,7 @@ class Ekf final : public EstimatorInterface
#if defined(CONFIG_EKF2_WIND)
// perform a reset of the wind states and related covariances
void resetWind();
void resetWindCov();
void resetWindToZero();
#endif // CONFIG_EKF2_WIND

Expand Down
Loading