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

ekf2: preserve max variance ratio across all states

Browse Source
This commit is contained in:
Daniel Agar
2024-07-25 16:45:21 -04:00
parent 85b6b0a406
commit 7c2eb9aa27
3 changed files with 18 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp
+2 -2
View File
@@ -234,8 +234,8 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
if (!_control_status.flags.yaw_align
|| wmm_updated
|| !_state.mag_I.longerThan(0.f)
|| (getStateVariance<State::mag_I>().min() < kMagVarianceMin)
|| (getStateVariance<State::mag_B>().min() < kMagVarianceMin)
|| (getStateVariance<State::mag_I>().min() < FLT_EPSILON)
|| (getStateVariance<State::mag_B>().min() < FLT_EPSILON)
) {
ECL_INFO("starting %s fusion, resetting states", AID_SRC_NAME);
src/modules/ekf2/EKF/covariance.cpp
+16 -37
View File
@@ -247,32 +247,30 @@ void Ekf::constrainStateVariances()
// Covariance diagonal limits. Use same values for states which
// belong to the same group (e.g. vel_x, vel_y, vel_z)
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)
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)
if (_control_status.flags.wind) {
constrainStateVarLimitRatio(State::wind_vel, 1e-6f, 1e6f);
}
constrainStateVar(State::wind_vel, 1e-6f, 1e6f);
#endif // CONFIG_EKF2_WIND
#if defined(CONFIG_EKF2_TERRAIN)
constrainStateVarLimitRatio(State::terrain, 0.f, 1e4f);
constrainStateVar(State::terrain, 1e-6f, 1e6f);
#endif // CONFIG_EKF2_TERRAIN
static constexpr float kMaxVarRatio = 1e8f;
float max_var = P.diag().max();
float min_var = max_var / kMaxVarRatio;
for (unsigned row = 0; row < State::size; row++) {
unsigned column = row;
if (P(row, column) < min_var) {
ECL_DEBUG("constraining P(%d,%d) %.8f->%.8f", row, column, (double)P(row, column), (double)min_var);
P(row, column) = min_var;
}
}
}
void Ekf::constrainStateVar(const IdxDof &state, float min, float max)
@@ -282,25 +280,6 @@ 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);
}
}
void Ekf::resetQuatCov(const float yaw_noise)
{
const float tilt_var = sq(math::max(_params.initial_tilt_err, 0.01f));
src/modules/ekf2/EKF/ekf.h
-9
View File
@@ -554,14 +554,6 @@ private:
void updateHorizontalDeadReckoningstatus();
void updateVerticalDeadReckoningStatus();
static constexpr float kGyroBiasVarianceMin{1e-9f};
static constexpr float kAccelBiasVarianceMin{1e-9f};
#if defined(CONFIG_EKF2_MAGNETOMETER)
static constexpr float kMagVarianceMin = 1e-6f;
#endif // CONFIG_EKF2_MAGNETOMETER
struct StateResetCounts {
uint8_t velNE{0}; ///< number of horizontal position reset events (allow to wrap if count exceeds 255)
uint8_t velD{0}; ///< number of vertical velocity reset events (allow to wrap if count exceeds 255)
@@ -940,7 +932,6 @@ private:
void constrainStateVariances();
void constrainStateVar(const IdxDof &state, float min, float max);
void constrainStateVarLimitRatio(const IdxDof &state, float min, float max, float max_ratio = 1.e6f);
// generic function which will perform a fusion step given a kalman gain K
// and a scalar innovation value