ekf2: add vel/pos/terr covariance reset helpers

src/modules/ekf2/EKF/covariance.cpp

@@ -54,45 +54,9 @@ void Ekf::initialiseCovariance()
 	P.zero();
 
 	resetQuatCov(0.f); // Start with no initial uncertainty to improve fine leveling through zero vel/pos fusion
-
-	// velocity
-#if defined(CONFIG_EKF2_GNSS)
-	const float vel_var = sq(fmaxf(_params.gps_vel_noise, 0.01f));
-#else
-	const float vel_var = sq(0.5f);
-#endif
-	P.uncorrelateCovarianceSetVariance<State::vel.dof>(State::vel.idx, Vector3f(vel_var, vel_var, sq(1.5f) * vel_var));
-
-	// position
-#if defined(CONFIG_EKF2_BAROMETER)
-	float z_pos_var = sq(fmaxf(_params.baro_noise, 0.01f));
-#else
-	float z_pos_var = sq(1.f);
-#endif // CONFIG_EKF2_BAROMETER
-
-#if defined(CONFIG_EKF2_GNSS)
-	const float xy_pos_var = sq(fmaxf(_params.gps_pos_noise, 0.01f));
-
-	if (_control_status.flags.gps_hgt) {
-		z_pos_var = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f));
-	}
-
-#else
-	const float xy_pos_var = sq(fmaxf(_params.pos_noaid_noise, 0.01f));
-#endif
-
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-
-	if (_control_status.flags.rng_hgt) {
-		z_pos_var = sq(fmaxf(_params.range_noise, 0.01f));
-	}
-
-#endif // CONFIG_EKF2_RANGE_FINDER
-
-	P.uncorrelateCovarianceSetVariance<State::pos.dof>(State::pos.idx, Vector3f(xy_pos_var, xy_pos_var, z_pos_var));
-
+	resetVelocityCov();
+	resetPositionCov();
 	resetGyroBiasCov();
-
 	resetAccelBiasCov();
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
@@ -104,8 +68,7 @@ void Ekf::initialiseCovariance()
 #endif // CONFIG_EKF2_WIND
 
 #if defined(CONFIG_EKF2_TERRAIN)
-	// use the ground clearance value as our uncertainty
-	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, sq(_params.rng_gnd_clearance));
+	resetTerrainCov();
 #endif // CONFIG_EKF2_TERRAIN
 }
 

src/modules/ekf2/EKF/ekf.h

@@ -110,6 +110,8 @@ public:
 	// get the terrain variance
 	float getTerrainVariance() const { return P(State::terrain.idx, State::terrain.idx); }
 
+	void resetTerrainCov();
+
 #endif // CONFIG_EKF2_TERRAIN
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
@@ -796,12 +798,17 @@ private:
 
 	void resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var);
 
+	void resetVelocityCov();
+
 
 	void resetHorizontalPositionToLastKnown();
 
 	void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var);
 	void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const float pos_var = NAN) { resetHorizontalPositionTo(new_horz_pos, Vector2f(pos_var, pos_var)); }
 
+	void resetPositionCov();
+
+
 	bool isHeightResetRequired() const;
 
 	void resetVerticalPositionTo(float new_vert_pos, float new_vert_pos_var = NAN);

src/modules/ekf2/EKF/position_fusion.cpp

@@ -181,3 +181,16 @@ void Ekf::resetHorizontalPositionToLastKnown()
 	// Used when falling back to non-aiding mode of operation
 	resetHorizontalPositionTo(_last_known_pos.xy(), sq(_params.pos_noaid_noise));
 }
+
+void Ekf::resetPositionCov()
+{
+	// reset position covariances
+	ECL_INFO("resetting position covariances");
+
+	float xy_var = sq(math::max(_params.pos_noaid_noise, 0.01f));
+	float z_var = sq(1.f);
+
+	Vector3f pos_var{xy_var, xy_var, z_var};
+
+	P.uncorrelateCovarianceSetVariance<State::pos.dof>(State::pos.idx, pos_var);
+}

src/modules/ekf2/EKF/terrain_control.cpp

@@ -45,8 +45,7 @@ void Ekf::initTerrain()
 	// assume a ground clearance
 	_state.terrain = _state.pos(2) + _params.rng_gnd_clearance;
 
-	// use the ground clearance value as our uncertainty
-	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, sq(_params.rng_gnd_clearance));
+	resetTerrainCov();
 }
 
 void Ekf::controlTerrainFakeFusion()
@@ -101,3 +100,12 @@ bool Ekf::isTerrainEstimateValid() const
 
 	return valid;
 }
+
+void Ekf::resetTerrainCov()
+{
+	// reset terrain covariances
+
+	// use the ground clearance value as our uncertainty
+	float terrain_var = math::max(sq(_params.rng_gnd_clearance), sq(0.01f));
+	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, terrain_var);
+}

src/modules/ekf2/EKF/velocity_fusion.cpp

@@ -156,3 +156,15 @@ void Ekf::resetVelocityTo(const Vector3f &new_vel, const Vector3f &new_vel_var)
 	resetHorizontalVelocityTo(Vector2f(new_vel), Vector2f(new_vel_var(0), new_vel_var(1)));
 	resetVerticalVelocityTo(new_vel(2), new_vel_var(2));
 }
+
+void Ekf::resetVelocityCov()
+{
+	// reset velocity covariances
+	ECL_INFO("resetting velocity covariances");
+
+	float default_var = sq(1.f); // TODO: configurable?
+
+	Vector3f vel_var{default_var, default_var, default_var};
+
+	P.uncorrelateCovarianceSetVariance<State::vel.dof>(State::vel.idx, vel_var);
+}