mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-18 16:23:33 +08:00
ekf2: treat inhibited states as "consider states" and remove conservative covariance matrix fixes (#22597)
When a state stops to be estimated it becomes a "consider state". Its value and variance are frozen but its covariance with other states continue to evolve normally. - removes conservative accel bias variance limiting - force symmetry is skipped after fusion of NED vel/pos (a direct measurement) --------- Co-authored-by: bresch <brescianimathieu@gmail.com>
This commit is contained in:
Daniel Agar
@@ -103,14 +103,14 @@ void Ekf::initialiseCovariance()
|
||||
|
||||
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;
|
||||
// predict the covariance
|
||||
const float dt = 0.5f * (imu_delayed.delta_vel_dt + imu_delayed.delta_ang_dt);
|
||||
|
||||
// delta angle noise variance
|
||||
// gyro noise variance
|
||||
float gyro_noise = math::constrain(_params.gyro_noise, 0.f, 1.f);
|
||||
const float gyro_var = sq(gyro_noise);
|
||||
|
||||
// delta velocity noise variance
|
||||
// accel noise variance
|
||||
float accel_noise = math::constrain(_params.accel_noise, 0.f, 1.f);
|
||||
Vector3f accel_var;
|
||||
|
||||
@@ -124,211 +124,112 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
|
||||
}
|
||||
}
|
||||
|
||||
// predict the covariance
|
||||
// calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states
|
||||
P = sym::PredictCovariance(_state.vector(), P,
|
||||
imu_delayed.delta_vel / math::max(imu_delayed.delta_vel_dt, FLT_EPSILON), accel_var,
|
||||
imu_delayed.delta_ang / math::max(imu_delayed.delta_ang_dt, FLT_EPSILON), gyro_var,
|
||||
0.5f * (imu_delayed.delta_vel_dt + imu_delayed.delta_ang_dt));
|
||||
imu_delayed.delta_vel / math::max(imu_delayed.delta_vel_dt, FLT_EPSILON), accel_var,
|
||||
imu_delayed.delta_ang / math::max(imu_delayed.delta_ang_dt, FLT_EPSILON), gyro_var,
|
||||
dt);
|
||||
|
||||
// Construct the process noise variance diagonal for those states with a stationary process model
|
||||
// These are kinematic states and their error growth is controlled separately by the IMU noise variances
|
||||
|
||||
// gyro bias: add process noise, or restore previous gyro bias var if state inhibited
|
||||
const float gyro_bias_sig = dt * math::constrain(_params.gyro_bias_p_noise, 0.f, 1.f);
|
||||
const float gyro_bias_process_noise = sq(gyro_bias_sig);
|
||||
for (unsigned index = 0; index < State::gyro_bias.dof; index++) {
|
||||
const unsigned i = State::gyro_bias.idx + index;
|
||||
// gyro bias: add process noise unless state is inhibited
|
||||
{
|
||||
const float gyro_bias_sig = dt * math::constrain(_params.gyro_bias_p_noise, 0.f, 1.f);
|
||||
const float gyro_bias_process_noise = sq(gyro_bias_sig);
|
||||
|
||||
if (!_gyro_bias_inhibit[index]) {
|
||||
P(i, i) += gyro_bias_process_noise;
|
||||
for (unsigned index = 0; index < State::gyro_bias.dof; index++) {
|
||||
const unsigned i = State::gyro_bias.idx + index;
|
||||
|
||||
} else {
|
||||
P.uncorrelateCovarianceSetVariance<1>(i, _prev_gyro_bias_var(index));
|
||||
if (!_gyro_bias_inhibit[index]) {
|
||||
P(i, i) += gyro_bias_process_noise;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// accel bias: add process noise, or restore previous accel bias var if state inhibited
|
||||
const float accel_bias_sig = dt * math::constrain(_params.accel_bias_p_noise, 0.f, 1.f);
|
||||
const float accel_bias_process_noise = sq(accel_bias_sig);
|
||||
for (unsigned index = 0; index < State::accel_bias.dof; index++) {
|
||||
const unsigned i = State::accel_bias.idx + index;
|
||||
// accel bias: add process noise unless state is inhibited
|
||||
{
|
||||
const float accel_bias_sig = dt * math::constrain(_params.accel_bias_p_noise, 0.f, 1.f);
|
||||
const float accel_bias_process_noise = sq(accel_bias_sig);
|
||||
|
||||
if (!_accel_bias_inhibit[index]) {
|
||||
P(i, i) += accel_bias_process_noise;
|
||||
for (unsigned index = 0; index < State::accel_bias.dof; index++) {
|
||||
const unsigned i = State::accel_bias.idx + index;
|
||||
|
||||
} else {
|
||||
P.uncorrelateCovarianceSetVariance<1>(i, _prev_accel_bias_var(index));
|
||||
if (!_accel_bias_inhibit[index]) {
|
||||
P(i, i) += accel_bias_process_noise;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
if (_control_status.flags.mag) {
|
||||
// Don't continue to grow the earth field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
|
||||
if (P.trace<State::mag_I.dof>(State::mag_I.idx) < 0.1f) {
|
||||
// mag_I: add process noise
|
||||
float mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.f, 1.f);
|
||||
P.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0) += sq(mag_I_sig);
|
||||
|
||||
float mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.f, 1.f);
|
||||
float mag_I_process_noise = sq(mag_I_sig);
|
||||
|
||||
for (unsigned index = 0; index < State::mag_I.dof; index++) {
|
||||
unsigned i = State::mag_I.idx + index;
|
||||
P(i, i) += mag_I_process_noise;
|
||||
}
|
||||
}
|
||||
|
||||
// Don't continue to grow the body field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
|
||||
if (P.trace<State::mag_B.dof>(State::mag_B.idx) < 0.1f) {
|
||||
|
||||
float mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.f, 1.f);
|
||||
float mag_B_process_noise = sq(mag_B_sig);
|
||||
|
||||
for (unsigned index = 0; index < State::mag_B.dof; index++) {
|
||||
unsigned i = State::mag_B.idx + index;
|
||||
P(i, i) += mag_B_process_noise;
|
||||
}
|
||||
}
|
||||
// mag_B: add process noise
|
||||
float mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.f, 1.f);
|
||||
P.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) += sq(mag_B_sig);
|
||||
}
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
|
||||
#if defined(CONFIG_EKF2_WIND)
|
||||
if (_control_status.flags.wind) {
|
||||
// Don't continue to grow wind velocity state variances if they are becoming too large or we are not using wind velocity states as this can make the covariance matrix badly conditioned
|
||||
if (P.trace<State::wind_vel.dof>(State::wind_vel.idx) < sq(_params.initial_wind_uncertainty)) {
|
||||
|
||||
float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f, 1.f) * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf));
|
||||
|
||||
const float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt;
|
||||
|
||||
for (unsigned index = 0; index < State::wind_vel.dof; index++) {
|
||||
unsigned i = State::wind_vel.idx + index;
|
||||
P(i, i) += wind_vel_process_noise;
|
||||
}
|
||||
}
|
||||
// wind vel: add process noise
|
||||
float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f, 1.f) * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf));
|
||||
P.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) += sq(wind_vel_nsd_scaled) * dt;
|
||||
}
|
||||
#endif // CONFIG_EKF2_WIND
|
||||
|
||||
|
||||
// covariance matrix is symmetrical, so copy upper half to lower half
|
||||
for (unsigned row = 0; row < State::size; row++) {
|
||||
for (unsigned column = 0 ; column < row; column++) {
|
||||
for (unsigned column = 0; column < row; column++) {
|
||||
P(row, column) = P(column, row);
|
||||
}
|
||||
}
|
||||
|
||||
// fix gross errors in the covariance matrix and ensure rows and
|
||||
// columns for un-used states are zero
|
||||
fixCovarianceErrors(false);
|
||||
constrainStateVariances();
|
||||
}
|
||||
|
||||
void Ekf::fixCovarianceErrors(bool force_symmetry)
|
||||
void Ekf::constrainStateVariances()
|
||||
{
|
||||
// NOTE: This limiting is a last resort and should not be relied on
|
||||
// TODO: Split covariance prediction into separate F*P*transpose(F) and Q contributions
|
||||
// and set corresponding entries in Q to zero when states exceed 50% of the limit
|
||||
// Covariance diagonal limits. Use same values for states which
|
||||
// belong to the same group (e.g. vel_x, vel_y, vel_z)
|
||||
const float quat_var_max = 1.0f;
|
||||
const float vel_var_max = 1e6f;
|
||||
const float pos_var_max = 1e6f;
|
||||
const float gyro_bias_var_max = 1.0f;
|
||||
|
||||
constrainStateVar(State::quat_nominal, 0.f, quat_var_max);
|
||||
constrainStateVar(State::vel, 1e-6f, vel_var_max);
|
||||
constrainStateVar(State::pos, 1e-6f, pos_var_max);
|
||||
constrainStateVar(State::gyro_bias, 0.f, gyro_bias_var_max);
|
||||
|
||||
// the following states are optional and are deactivated when not required
|
||||
// by ensuring the corresponding covariance matrix values are kept at zero
|
||||
|
||||
// accelerometer bias states
|
||||
if (!_accel_bias_inhibit[0] || !_accel_bias_inhibit[1] || !_accel_bias_inhibit[2]) {
|
||||
// Find the maximum delta velocity bias state variance and request a covariance reset if any variance is below the safe minimum
|
||||
const float minSafeStateVar = 1e-9f / sq(_dt_ekf_avg);
|
||||
float maxStateVar = minSafeStateVar;
|
||||
bool resetRequired = false;
|
||||
|
||||
for (unsigned axis = 0; axis < State::accel_bias.dof; axis++) {
|
||||
const unsigned stateIndex = State::accel_bias.idx + axis;
|
||||
|
||||
if (_accel_bias_inhibit[axis]) {
|
||||
// Skip the check for the inhibited axis
|
||||
continue;
|
||||
}
|
||||
|
||||
if (P(stateIndex, stateIndex) > maxStateVar) {
|
||||
maxStateVar = P(stateIndex, stateIndex);
|
||||
|
||||
} else if (P(stateIndex, stateIndex) < minSafeStateVar) {
|
||||
resetRequired = true;
|
||||
}
|
||||
}
|
||||
|
||||
// To ensure stability of the covariance matrix operations, the ratio of a max and min variance must
|
||||
// not exceed 100 and the minimum variance must not fall below the target minimum
|
||||
// Also limit variance to a maximum equivalent to a 0.1g uncertainty
|
||||
const float minStateVarTarget = 5E-8f / sq(_dt_ekf_avg);
|
||||
float minAllowedStateVar = fmaxf(0.01f * maxStateVar, minStateVarTarget);
|
||||
|
||||
for (unsigned axis = 0; axis < State::accel_bias.dof; axis++) {
|
||||
const unsigned stateIndex = State::accel_bias.idx + axis;
|
||||
|
||||
if (_accel_bias_inhibit[axis]) {
|
||||
// Skip the check for the inhibited axis
|
||||
continue;
|
||||
}
|
||||
|
||||
P(stateIndex, stateIndex) = math::constrain(P(stateIndex, stateIndex), minAllowedStateVar, sq(0.1f * CONSTANTS_ONE_G));
|
||||
}
|
||||
|
||||
// If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero
|
||||
if (resetRequired) {
|
||||
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);
|
||||
}
|
||||
constrainStateVar(State::quat_nominal, 1e-9f, 1.f);
|
||||
constrainStateVar(State::vel, 1e-6f, 1e6f);
|
||||
constrainStateVar(State::pos, 1e-6f, 1e6f);
|
||||
constrainStateVarLimitRatio(State::gyro_bias, kGyroBiasVarianceMin, 1.f);
|
||||
constrainStateVarLimitRatio(State::accel_bias, kAccelBiasVarianceMin, 1.f);
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
// magnetic field states
|
||||
if (!_control_status.flags.mag) {
|
||||
P.uncorrelateCovarianceSetVariance<State::mag_I.dof>(State::mag_I.idx, 0.0f);
|
||||
P.uncorrelateCovarianceSetVariance<State::mag_B.dof>(State::mag_B.idx, 0.0f);
|
||||
|
||||
} else {
|
||||
const float mag_I_var_max = 1.f;
|
||||
constrainStateVar(State::mag_I, 0.f, mag_I_var_max);
|
||||
|
||||
const float mag_B_var_max = 1.f;
|
||||
constrainStateVar(State::mag_B, 0.f, mag_B_var_max);
|
||||
|
||||
if (force_symmetry) {
|
||||
P.makeRowColSymmetric<State::mag_I.dof>(State::mag_I.idx);
|
||||
P.makeRowColSymmetric<State::mag_B.dof>(State::mag_B.idx);
|
||||
}
|
||||
if (_control_status.flags.mag) {
|
||||
constrainStateVarLimitRatio(State::mag_I, kMagVarianceMin, 1.f);
|
||||
constrainStateVarLimitRatio(State::mag_B, kMagVarianceMin, 1.f);
|
||||
}
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_WIND)
|
||||
// wind velocity states
|
||||
if (!_control_status.flags.wind) {
|
||||
P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, 0.0f);
|
||||
|
||||
} else {
|
||||
const float wind_vel_var_max = 1e6f;
|
||||
constrainStateVar(State::wind_vel, 0.f, wind_vel_var_max);
|
||||
|
||||
if (force_symmetry) {
|
||||
P.makeRowColSymmetric<State::wind_vel.dof>(State::wind_vel.idx);
|
||||
}
|
||||
if (_control_status.flags.wind) {
|
||||
constrainStateVarLimitRatio(State::wind_vel, 1e-6f, 1e6f);
|
||||
}
|
||||
#endif // CONFIG_EKF2_WIND
|
||||
}
|
||||
|
||||
void Ekf::forceCovarianceSymmetry()
|
||||
{
|
||||
// DEBUG
|
||||
// P.isBlockSymmetric(0, 1e-9f);
|
||||
|
||||
P.makeRowColSymmetric<State::size>(0);
|
||||
}
|
||||
|
||||
void Ekf::constrainStateVar(const IdxDof &state, float min, float max)
|
||||
{
|
||||
for (unsigned i = state.idx; i < (state.idx + state.dof); i++) {
|
||||
@@ -336,6 +237,25 @@ void Ekf::constrainStateVar(const IdxDof &state, float min, float max)
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::constrainStateVarLimitRatio(const IdxDof &state, float min, float max, float max_ratio)
|
||||
{
|
||||
// the ratio of a max and min variance must not exceed max_ratio
|
||||
float state_var_max = 0.f;
|
||||
|
||||
for (unsigned i = state.idx; i < (state.idx + state.dof); i++) {
|
||||
if (P(i, i) > state_var_max) {
|
||||
state_var_max = P(i, i);
|
||||
}
|
||||
}
|
||||
|
||||
float limited_max = math::constrain(state_var_max, min, max);
|
||||
float limited_min = math::constrain(limited_max / max_ratio, min, max);
|
||||
|
||||
for (unsigned i = state.idx; i < (state.idx + state.dof); i++) {
|
||||
P(i, i) = math::constrain(P(i, i), limited_min, limited_max);
|
||||
}
|
||||
}
|
||||
|
||||
// if the covariance correction will result in a negative variance, then
|
||||
// the covariance matrix is unhealthy and must be corrected
|
||||
bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrixState &KHP)
|
||||
@@ -344,7 +264,7 @@ bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrixState &KHP)
|
||||
|
||||
for (int i = 0; i < State::size; i++) {
|
||||
if (P(i, i) < KHP(i, i)) {
|
||||
P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
|
||||
P.uncorrelateCovarianceSetVariance<1>(i, 0.f);
|
||||
healthy = false;
|
||||
}
|
||||
}
|
||||
@@ -382,14 +302,10 @@ void Ekf::resetMagCov()
|
||||
|
||||
P.uncorrelateCovarianceSetVariance<State::mag_I.dof>(State::mag_I.idx, sq(_params.mag_noise));
|
||||
P.uncorrelateCovarianceSetVariance<State::mag_B.dof>(State::mag_B.idx, sq(_params.mag_noise));
|
||||
|
||||
saveMagCovData();
|
||||
}
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
void Ekf::resetGyroBiasZCov()
|
||||
{
|
||||
const float init_gyro_bias_var = sq(_params.switch_on_gyro_bias);
|
||||
|
||||
P.uncorrelateCovarianceSetVariance<1>(State::gyro_bias.idx + 2, init_gyro_bias_var);
|
||||
P.uncorrelateCovarianceSetVariance<1>(State::gyro_bias.idx + 2, sq(_params.switch_on_gyro_bias));
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user