ekf2: use Joseph stabilized covariance update

src/modules/ekf2/EKF/covariance.cpp

@@ -222,14 +222,6 @@ void Ekf::constrainStateVariances()
 #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++) {
@@ -256,22 +248,6 @@ void Ekf::constrainStateVarLimitRatio(const IdxDof &state, float min, float 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)
-{
-	bool healthy = true;
-
-	for (int i = 0; i < State::size; i++) {
-		if (P(i, i) < KHP(i, i)) {
-			P.uncorrelateCovarianceSetVariance<1>(i, 0.f);
-			healthy = false;
-		}
-	}
-
-	return healthy;
-}
-
 void Ekf::resetQuatCov(const float yaw_noise)
 {
 	const float tilt_var = sq(math::max(_params.initial_tilt_err, 0.01f));