ekf2: preserve max variance ratio across all states

src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp

@@ -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

@@ -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

@@ -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