ekf2: move zero gyro update to aid source class

src/modules/ekf2/EKF/ekf.h

@@ -59,6 +59,7 @@
 #include <uORB/topics/estimator_aid_source2d.h>
 #include <uORB/topics/estimator_aid_source3d.h>
 
+#include "aid_sources/ZeroGyroUpdate.hpp"
 #include "aid_sources/ZeroVelocityUpdate.hpp"
 
 enum class Likelihood { LOW, MEDIUM, HIGH };
@@ -249,6 +250,7 @@ public:
 
 	// get the full covariance matrix
 	const matrix::SquareMatrix<float, State::size> &covariances() const { return P; }
+	float stateCovariance(unsigned r, unsigned c) const { return P(r, c); }
 
 	// get the diagonal elements of the covariance matrix
 	matrix::Vector<float, State::size> covariances_diagonal() const { return P.diag(); }
@@ -328,6 +330,7 @@ public:
 	const Vector3f &getGyroBias() const { return _state.gyro_bias; } // get the gyroscope bias in rad/s
 	Vector3f getGyroBiasVariance() const { return getStateVariance<State::gyro_bias>(); } // get the gyroscope bias variance in rad/s
 	float getGyroBiasLimit() const { return _params.gyro_bias_lim; }
+	float getGyroNoise() const { return _params.gyro_noise; }
 
 	// accel bias
 	const Vector3f &getAccelBias() const { return _state.accel_bias; } // get the accelerometer bias in m/s**2
@@ -473,6 +476,37 @@ public:
 	const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; }
 #endif // CONFIG_EKF2_AUXVEL
 
+
+	bool measurementUpdate(VectorState &K, float innovation_variance, float innovation)
+	{
+		clearInhibitedStateKalmanGains(K);
+
+		const VectorState KS = K * innovation_variance;
+		SquareMatrixState KHP;
+
+		for (unsigned row = 0; row < State::size; row++) {
+			for (unsigned col = 0; col < State::size; col++) {
+				// Instad of literally computing KHP, use an equvalent
+				// equation involving less mathematical operations
+				KHP(row, col) = KS(row) * K(col);
+			}
+		}
+
+		const bool is_healthy = checkAndFixCovarianceUpdate(KHP);
+
+		if (is_healthy) {
+			// apply the covariance corrections
+			P -= KHP;
+
+			fixCovarianceErrors(true);
+
+			// apply the state corrections
+			fuse(K, innovation);
+		}
+
+		return is_healthy;
+	}
+
 private:
 
 	// set the internal states and status to their default value
@@ -530,10 +564,6 @@ private:
 
 	Dcmf _R_to_earth{};	///< transformation matrix from body frame to earth frame from last EKF prediction
 
-	// zero gyro update
-	Vector3f _zgup_delta_ang{};
-	float _zgup_delta_ang_dt{0.f};
-
 	Vector2f _accel_lpf_NE{};			///< Low pass filtered horizontal earth frame acceleration (m/sec**2)
 	float _height_rate_lpf{0.0f};
 	float _yaw_delta_ef{0.0f};		///< Recent change in yaw angle measured about the earth frame D axis (rad)
@@ -917,36 +947,6 @@ private:
 #endif // CONFIG_EKF2_WIND
 	}
 
-	bool measurementUpdate(VectorState &K, float innovation_variance, float innovation)
-	{
-		clearInhibitedStateKalmanGains(K);
-
-		const VectorState KS = K * innovation_variance;
-		SquareMatrixState KHP;
-
-		for (unsigned row = 0; row < State::size; row++) {
-			for (unsigned col = 0; col < State::size; col++) {
-				// Instad of literally computing KHP, use an equvalent
-				// equation involving less mathematical operations
-				KHP(row, col) = KS(row) * K(col);
-			}
-		}
-
-		const bool is_healthy = checkAndFixCovarianceUpdate(KHP);
-
-		if (is_healthy) {
-			// apply the covariance corrections
-			P -= KHP;
-
-			fixCovarianceErrors(true);
-
-			// apply the state corrections
-			fuse(K, innovation);
-		}
-
-		return is_healthy;
-	}
-
 	// if the covariance correction will result in a negative variance, then
 	// the covariance matrix is unhealthy and must be corrected
 	bool checkAndFixCovarianceUpdate(const SquareMatrixState &KHP);
@@ -1073,9 +1073,6 @@ private:
 	void resetHeightToLastKnown();
 	void stopFakeHgtFusion();
 
-	void controlZeroGyroUpdate(const imuSample &imu_delayed);
-	void fuseDeltaAngBias(float innov, float innov_var, int obs_index);
-
 	void controlZeroInnovationHeadingUpdate();
 
 #if defined(CONFIG_EKF2_AUXVEL)
@@ -1242,6 +1239,7 @@ private:
 		status.innovation_rejected = innovation_rejected;
 	}
 
+	ZeroGyroUpdate _zero_gyro_update{};
 	ZeroVelocityUpdate _zero_velocity_update{};
 };