EKF: remove virtual getters from estimator_interface

EKF/airspeed_fusion.cpp

@@ -161,12 +161,7 @@ void Ekf::fuseAirspeed()
 	}
 }
 
-Vector2f Ekf::getWindVelocityVariance() const
+void Ekf::get_true_airspeed(float *tas) const
-{
-	return P.slice<2, 2>(22,22).diag();
-}
-
-void Ekf::get_true_airspeed(float *tas)
 {
 	const float tempvar = sqrtf(sq(_state.vel(0) - _state.wind_vel(0)) + sq(_state.vel(1) - _state.wind_vel(1)) + sq(_state.vel(2)));
 	memcpy(tas, &tempvar, sizeof(float));

EKF/control.cpp

@@ -77,7 +77,7 @@ void Ekf::controlFusionModes()
 				height_source = "unknown";
 
 			}
-			if(height_source){
+			if (height_source){
 				ECL_INFO("%llu: EKF aligned, (%s hgt, IMU buf: %i, OBS buf: %i)",
 					(unsigned long long)_imu_sample_delayed.time_us, height_source, (int)_imu_buffer_length, (int)_obs_buffer_length);
 			}

EKF/covariance.cpp

@@ -1034,7 +1034,6 @@ bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrix24f& KHP) {
 	return healthy;
 }
 
-
 void Ekf::resetMagRelatedCovariances()
 {
 	resetQuatCov();
@@ -1119,6 +1118,5 @@ void Ekf::resetWindCovariance()
 	} else {
 		// without airspeed, start with a small initial uncertainty to improve the initial estimate
 		P.uncorrelateCovarianceSetVariance<2>(22, _params.initial_wind_uncertainty);
-
 	}
 }

EKF/ekf.h

@@ -44,6 +44,8 @@
 
 #include "estimator_interface.h"
 
+#include "EKFGSF_yaw.h"
+
 class Ekf final : public EstimatorInterface
 {
 public:
@@ -61,78 +63,73 @@ public:
 	// initialise variables to sane values (also interface class)
 	bool init(uint64_t timestamp) override;
 
-	// set the internal states and status to their default value
-	void reset() override;
-
-	bool initialiseTilt();
-
 	// should be called every time new data is pushed into the filter
-	bool update() override;
+	bool update();
 
-	void getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const override;
+	void getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
-	void getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const override;
+	void getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
-	void getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const override;
+	void getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const;
 
-	void getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const override;
+	void getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
-	void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const override;
+	void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
-	void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const override;
+	void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const;
 
-	void getBaroHgtInnov(float &baro_hgt_innov) const override { baro_hgt_innov = _baro_hgt_innov(2); }
+	void getBaroHgtInnov(float &baro_hgt_innov) const { baro_hgt_innov = _baro_hgt_innov(2); }
-	void getBaroHgtInnovVar(float &baro_hgt_innov_var) const override { baro_hgt_innov_var = _baro_hgt_innov_var(2); }
+	void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _baro_hgt_innov_var(2); }
-	void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const override { baro_hgt_innov_ratio = _baro_hgt_test_ratio(1); }
+	void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const { baro_hgt_innov_ratio = _baro_hgt_test_ratio(1); }
 
-	void getRngHgtInnov(float &rng_hgt_innov) const override { rng_hgt_innov = _rng_hgt_innov(2); }
+	void getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _rng_hgt_innov(2); }
-	void getRngHgtInnovVar(float &rng_hgt_innov_var) const override { rng_hgt_innov_var = _rng_hgt_innov_var(2); }
+	void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _rng_hgt_innov_var(2); }
-	void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const override { rng_hgt_innov_ratio = _rng_hgt_test_ratio(1); }
+	void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _rng_hgt_test_ratio(1); }
 
-	void getAuxVelInnov(float aux_vel_innov[2]) const override;
+	void getAuxVelInnov(float aux_vel_innov[2]) const;
-	void getAuxVelInnovVar(float aux_vel_innov[2]) const override;
+	void getAuxVelInnovVar(float aux_vel_innov[2]) const;
-	void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const override { aux_vel_innov_ratio = _aux_vel_test_ratio(0); }
+	void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const { aux_vel_innov_ratio = _aux_vel_test_ratio(0); }
 
-	void getFlowInnov(float flow_innov[2]) const override { _flow_innov.copyTo(flow_innov); }
+	void getFlowInnov(float flow_innov[2]) const { _flow_innov.copyTo(flow_innov); }
-	void getFlowInnovVar(float flow_innov_var[2]) const override { _flow_innov_var.copyTo(flow_innov_var); }
+	void getFlowInnovVar(float flow_innov_var[2]) const { _flow_innov_var.copyTo(flow_innov_var); }
-	void getFlowInnovRatio(float &flow_innov_ratio) const override { flow_innov_ratio = _optflow_test_ratio; }
+	void getFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = _optflow_test_ratio; }
-	const Vector2f &getFlowVelBody() const override { return _flow_vel_body; }
+	const Vector2f &getFlowVelBody() const { return _flow_vel_body; }
-	const Vector2f &getFlowVelNE() const override { return _flow_vel_ne; }
+	const Vector2f &getFlowVelNE() const { return _flow_vel_ne; }
-	const Vector2f &getFlowCompensated() const override { return _flow_compensated_XY_rad; }
+	const Vector2f &getFlowCompensated() const { return _flow_compensated_XY_rad; }
-	const Vector2f &getFlowUncompensated() const override { return _flow_sample_delayed.flow_xy_rad; }
+	const Vector2f &getFlowUncompensated() const { return _flow_sample_delayed.flow_xy_rad; }
-	const Vector3f &getFlowGyro() const override { return _flow_sample_delayed.gyro_xyz; }
+	const Vector3f &getFlowGyro() const { return _flow_sample_delayed.gyro_xyz; }
 
-	void getHeadingInnov(float &heading_innov) const override { heading_innov = _heading_innov; }
+	void getHeadingInnov(float &heading_innov) const { heading_innov = _heading_innov; }
-	void getHeadingInnovVar(float &heading_innov_var) const override { heading_innov_var = _heading_innov_var; }
+	void getHeadingInnovVar(float &heading_innov_var) const { heading_innov_var = _heading_innov_var; }
 
-	void getHeadingInnovRatio(float &heading_innov_ratio) const override { heading_innov_ratio = _yaw_test_ratio; }
+	void getHeadingInnovRatio(float &heading_innov_ratio) const { heading_innov_ratio = _yaw_test_ratio; }
-	void getMagInnov(float mag_innov[3]) const override { _mag_innov.copyTo(mag_innov); }
+	void getMagInnov(float mag_innov[3]) const { _mag_innov.copyTo(mag_innov); }
-	void getMagInnovVar(float mag_innov_var[3]) const override { _mag_innov_var.copyTo(mag_innov_var); }
+	void getMagInnovVar(float mag_innov_var[3]) const { _mag_innov_var.copyTo(mag_innov_var); }
-	void getMagInnovRatio(float &mag_innov_ratio) const override { mag_innov_ratio = _mag_test_ratio.max(); }
+	void getMagInnovRatio(float &mag_innov_ratio) const { mag_innov_ratio = _mag_test_ratio.max(); }
 
-	void getDragInnov(float drag_innov[2]) const override { _drag_innov.copyTo(drag_innov); }
+	void getDragInnov(float drag_innov[2]) const { _drag_innov.copyTo(drag_innov); }
-	void getDragInnovVar(float drag_innov_var[2]) const override { _drag_innov_var.copyTo(drag_innov_var); }
+	void getDragInnovVar(float drag_innov_var[2]) const { _drag_innov_var.copyTo(drag_innov_var); }
-	void getDragInnovRatio(float drag_innov_ratio[2]) const override { _drag_test_ratio.copyTo(drag_innov_ratio); }
+	void getDragInnovRatio(float drag_innov_ratio[2]) const { _drag_test_ratio.copyTo(drag_innov_ratio); }
 
-	void getAirspeedInnov(float &airspeed_innov) const override { airspeed_innov = _airspeed_innov; }
+	void getAirspeedInnov(float &airspeed_innov) const { airspeed_innov = _airspeed_innov; }
-	void getAirspeedInnovVar(float &airspeed_innov_var) const override { airspeed_innov_var = _airspeed_innov_var; }
+	void getAirspeedInnovVar(float &airspeed_innov_var) const { airspeed_innov_var = _airspeed_innov_var; }
-	void getAirspeedInnovRatio(float &airspeed_innov_ratio) const override { airspeed_innov_ratio = _tas_test_ratio; }
+	void getAirspeedInnovRatio(float &airspeed_innov_ratio) const { airspeed_innov_ratio = _tas_test_ratio; }
 
-	void getBetaInnov(float &beta_innov) const override { beta_innov = _beta_innov; }
+	void getBetaInnov(float &beta_innov) const { beta_innov = _beta_innov; }
-	void getBetaInnovVar(float &beta_innov_var) const override { beta_innov_var = _beta_innov_var; }
+	void getBetaInnovVar(float &beta_innov_var) const { beta_innov_var = _beta_innov_var; }
-	void getBetaInnovRatio(float &beta_innov_ratio) const override { beta_innov_ratio = _beta_test_ratio; }
+	void getBetaInnovRatio(float &beta_innov_ratio) const { beta_innov_ratio = _beta_test_ratio; }
 
-	void getHaglInnov(float &hagl_innov) const override { hagl_innov = _hagl_innov; }
+	void getHaglInnov(float &hagl_innov) const { hagl_innov = _hagl_innov; }
-	void getHaglInnovVar(float &hagl_innov_var) const override { hagl_innov_var = _hagl_innov_var; }
+	void getHaglInnovVar(float &hagl_innov_var) const { hagl_innov_var = _hagl_innov_var; }
-	void getHaglInnovRatio(float &hagl_innov_ratio) const override { hagl_innov_ratio = _hagl_test_ratio; }
+	void getHaglInnovRatio(float &hagl_innov_ratio) const { hagl_innov_ratio = _hagl_test_ratio; }
 
 	// get the state vector at the delayed time horizon
-	matrix::Vector<float, 24> getStateAtFusionHorizonAsVector() const override;
+	matrix::Vector<float, 24> getStateAtFusionHorizonAsVector() const;
 
 	// get the wind velocity in m/s
 	const Vector2f &getWindVelocity() const { return _state.wind_vel; };
 
 	// get the wind velocity var
-	Vector2f getWindVelocityVariance() const override;
+	Vector2f getWindVelocityVariance() const { return P.slice<2, 2>(22,22).diag(); }
 
 	// get the true airspeed in m/s
-	void get_true_airspeed(float *tas) override;
+	void get_true_airspeed(float *tas) const;
 
 	// get the full covariance matrix
 	const matrix::SquareMatrix<float, 24> &covariances() const { return P; }
@@ -154,26 +151,26 @@ public:
 
 	// get the ekf WGS-84 origin position and height and the system time it was last set
 	// return true if the origin is valid
-	bool get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) override;
+	bool get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) const;
 
 	// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
-	void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) override;
+	void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const;
 
 	// get the 1-sigma horizontal and vertical position uncertainty of the ekf local position
-	void get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) override;
+	void get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const;
 
 	// get the 1-sigma horizontal and vertical velocity uncertainty
-	void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) override;
+	void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const;
 
 	// get the vehicle control limits required by the estimator to keep within sensor limitations
-	void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) override;
+	void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const;
 
 	/*
 	Reset all IMU bias states and covariances to initial alignment values.
 	Use when the IMU sensor has changed.
 	Returns true if reset performed, false if rejected due to less than 10 seconds lapsed since last reset.
 	*/
-	bool reset_imu_bias() override;
+	bool reset_imu_bias();
 
 	Vector3f getVelocityVariance() const { return P.slice<3, 3>(4, 4).diag(); };
 
@@ -183,14 +180,6 @@ public:
 	// error magnitudes (rad), (m/sec), (m)
 	const Vector3f &getOutputTrackingError() const { return _output_tracking_error; }
 
-	/*
-	Returns  following IMU vibration metrics in the following array locations
-	0 : Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle)
-	1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle)
-	2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity)
-	*/
-	const Vector3f &getImuVibrationMetrics() const { return _vibe_metrics; }
-
 	/*
 	First argument returns GPS drift  metrics in the following array locations
 	0 : Horizontal position drift rate (m/s)
@@ -199,22 +188,22 @@ public:
 	Second argument returns true when IMU movement is blocking the drift calculation
 	Function returns true if the metrics have been updated and not returned previously by this function
 	*/
-	bool get_gps_drift_metrics(float drift[3], bool *blocked) override;
+	bool get_gps_drift_metrics(float drift[3], bool *blocked);
 
 	// return true if the global position estimate is valid
-	bool global_position_is_valid() override;
+	// return true if the origin is set we are not doing unconstrained free inertial navigation
+	// and have not started using synthetic position observations to constrain drift
+	bool global_position_is_valid() const
+	{
+		return (_NED_origin_initialised && !_deadreckon_time_exceeded && !_using_synthetic_position);
+	}
 
-	// check if the EKF is dead reckoning horizontal velocity using inertial data only
+	bool isTerrainEstimateValid() const { return _hagl_valid; };
-	void update_deadreckoning_status();
 
-	bool isTerrainEstimateValid() const override;
+	uint8_t getTerrainEstimateSensorBitfield() const { return _hagl_sensor_status.value; }
-
-	uint8_t getTerrainEstimateSensorBitfield() const override {return _hagl_sensor_status.value;}
-
-	void updateTerrainValidity();
 
 	// get the estimated terrain vertical position relative to the NED origin
-	float getTerrainVertPos() const override;
+	float getTerrainVertPos() const { return _terrain_vpos; };
 
 	// get the terrain variance
 	float get_terrain_var() const { return _terrain_var; }
@@ -226,30 +215,38 @@ public:
 	Vector3f getGyroBias() const { return _state.delta_ang_bias / _dt_ekf_avg; }
 
 	// get GPS check status
-	void get_gps_check_status(uint16_t *val) override { *val = _gps_check_fail_status.value; }
+	void get_gps_check_status(uint16_t *val) const { *val = _gps_check_fail_status.value; }
 
 	// return the amount the local vertical position changed in the last reset and the number of reset events
-	void get_posD_reset(float *delta, uint8_t *counter) override {*delta = _state_reset_status.posD_change; *counter = _state_reset_status.posD_counter;}
+	void get_posD_reset(float *delta, uint8_t *counter) const
+	{
+		*delta = _state_reset_status.posD_change;
+		*counter = _state_reset_status.posD_counter;
+	}
 
 	// return the amount the local vertical velocity changed in the last reset and the number of reset events
-	void get_velD_reset(float *delta, uint8_t *counter) override {*delta = _state_reset_status.velD_change; *counter = _state_reset_status.velD_counter;}
+	void get_velD_reset(float *delta, uint8_t *counter) const
+	{
+		*delta = _state_reset_status.velD_change;
+		*counter = _state_reset_status.velD_counter;
+	}
 
 	// return the amount the local horizontal position changed in the last reset and the number of reset events
-	void get_posNE_reset(float delta[2], uint8_t *counter) override
+	void get_posNE_reset(float delta[2], uint8_t *counter) const
 	{
 		_state_reset_status.posNE_change.copyTo(delta);
 		*counter = _state_reset_status.posNE_counter;
 	}
 
 	// return the amount the local horizontal velocity changed in the last reset and the number of reset events
-	void get_velNE_reset(float delta[2], uint8_t *counter) override
+	void get_velNE_reset(float delta[2], uint8_t *counter) const
 	{
 		_state_reset_status.velNE_change.copyTo(delta);
 		*counter = _state_reset_status.velNE_counter;
 	}
 
 	// return the amount the quaternion has changed in the last reset and the number of reset events
-	void get_quat_reset(float delta_quat[4], uint8_t *counter) override
+	void get_quat_reset(float delta_quat[4], uint8_t *counter) const
 	{
 		_state_reset_status.quat_change.copyTo(delta_quat);
 		*counter = _state_reset_status.quat_counter;
@@ -261,10 +258,10 @@ public:
 	// A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF
 	// Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned.
 	void get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas,
-					float &hagl, float &beta) override;
+					float &hagl, float &beta) const;
 
 	// return a bitmask integer that describes which state estimates can be used for flight control
-	void get_ekf_soln_status(uint16_t *status) override;
+	void get_ekf_soln_status(uint16_t *status) const;
 
 	// return the quaternion defining the rotation from the External Vision to the EKF reference frame
 	matrix::Quatf getVisionAlignmentQuaternion() const { return Quatf(_R_ev_to_ekf); };
@@ -278,15 +275,26 @@ public:
 	// get solution data from the EKF-GSF emergency yaw estimator
 	// returns false when data is not available
 	bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
-			   float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) override;
+			   float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]);
+
+private:
+
+	// set the internal states and status to their default value
+	void reset();
+
+	bool initialiseTilt();
 
 	// Request the EKF reset the yaw to the estimate from the internal EKF-GSF filter
 	// and reset the velocity and position states to the GPS. This will cause the EKF
 	// to ignore the magnetometer for the remainder of flight.
 	// This should only be used as a last resort before activating a loss of navigation failsafe
-	void requestEmergencyNavReset() override { _do_ekfgsf_yaw_reset = true; }
+	void requestEmergencyNavReset() { _do_ekfgsf_yaw_reset = true; }
+
+	// check if the EKF is dead reckoning horizontal velocity using inertial data only
+	void update_deadreckoning_status();
+
+	void updateTerrainValidity();
 
-private:
 	struct {
 		uint8_t velNE_counter;	///< number of horizontal position reset events (allow to wrap if count exceeds 255)
 		uint8_t velD_counter;	///< number of vertical velocity reset events (allow to wrap if count exceeds 255)
@@ -620,8 +628,8 @@ private:
 	// return true if the initialisation is successful
 	bool initHagl();
 
-	bool shouldUseRangeFinderForHagl() const;
+	bool shouldUseRangeFinderForHagl() const { return (_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder); }
-	bool shouldUseOpticalFlowForHagl() const;
+	bool shouldUseOpticalFlowForHagl() const { return (_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseOpticalFlow); }
 
 	// run the terrain estimator
 	void runTerrainEstimator();
@@ -722,7 +730,7 @@ private:
 	// and a scalar innovation value
 	void fuse(const Vector24f &K, float innovation);
 
-	float compensateBaroForDynamicPressure(float baro_alt_uncompensated) override;
+	float compensateBaroForDynamicPressure(float baro_alt_uncompensated) const override;
 
 	// calculate the earth rotation vector from a given latitude
 	Vector3f calcEarthRateNED(float lat_rad) const;
@@ -750,28 +758,28 @@ private:
 	bool noOtherYawAidingThanMag() const;
 
 	void checkHaglYawResetReq();
-	float getTerrainVPos() const;
+	float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; }
 
 	void runOnGroundYawReset();
-	bool isYawResetAuthorized() const;
+	bool isYawResetAuthorized() const { return !_is_yaw_fusion_inhibited; }
 	bool canResetMagHeading() const;
 	void runInAirYawReset();
-	bool canRealignYawUsingGps() const;
+	bool canRealignYawUsingGps() const { return _control_status.flags.fixed_wing; }
 	void runVelPosReset();
 
 	void selectMagAuto();
 	void check3DMagFusionSuitability();
 	void checkYawAngleObservability();
 	void checkMagBiasObservability();
-	bool isYawAngleObservable() const;
+	bool isYawAngleObservable() const { return _yaw_angle_observable; }
-	bool isMagBiasObservable() const;
+	bool isMagBiasObservable() const { return _mag_bias_observable; }
 	bool canUse3DMagFusion() const;
 
 	void checkMagDeclRequired();
 	void checkMagInhibition();
 	bool shouldInhibitMag() const;
 	void checkMagFieldStrength();
-	bool isStrongMagneticDisturbance() const;
+	bool isStrongMagneticDisturbance() const { return _control_status.flags.mag_field_disturbed; }
 	bool isMeasuredMatchingGpsMagStrength() const;
 	bool isMeasuredMatchingAverageMagStrength() const;
 	static bool isMeasuredMatchingExpected(float measured, float expected, float gate);
@@ -809,7 +817,7 @@ private:
 
 	// determine if flight condition is suitable to use range finder instead of the primary height sensor
 	void checkRangeAidSuitability();
-	bool isRangeAidSuitable() { return _is_range_aid_suitable; }
+	bool isRangeAidSuitable() const { return _is_range_aid_suitable; }
 
 	// set control flags to use baro height
 	void setControlBaroHeight();
@@ -918,6 +926,9 @@ private:
 
 	// Declarations used to control use of the EKF-GSF yaw estimator
 
+	// yaw estimator instance
+	EKFGSF_yaw _yawEstimator;
+
 	int64_t _ekfgsf_yaw_reset_time{0};	///< timestamp of last emergency yaw reset (uSec)
 	uint64_t _time_last_on_ground_us{0};	///< last tine we were on the ground (uSec)
 	bool _do_ekfgsf_yaw_reset{false};	// true when an emergency yaw reset has been requested

EKF/ekf_helper.cpp

@@ -153,7 +153,6 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel)
 	_state_reset_status.velD_counter++;
 }
 
-
 void Ekf::resetHorizontalPosition()
 {
 	// let the next odometry update know that the previous value of states cannot be used to calculate the change in position
@@ -300,7 +299,6 @@ void Ekf::resetHeight()
 		} else {
 			_state.pos(2) = _ev_sample_delayed.pos(2);
 		}
-
 	}
 
 	// reset the vertical velocity state
@@ -574,7 +572,7 @@ void Ekf::constrainStates()
 	_state.wind_vel = matrix::constrain(_state.wind_vel, -100.0f, 100.0f);
 }
 
-float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated)
+float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const
 {
 	// calculate static pressure error = Pmeas - Ptruth
 	// model position error sensitivity as a body fixed ellipse with a different scale in the positive and
@@ -696,7 +694,7 @@ matrix::Vector<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
 
 // get the position and height of the ekf origin in WGS-84 coordinates and time the origin was set
 // return true if the origin is valid
-bool Ekf::get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt)
+bool Ekf::get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) const
 {
 	memcpy(origin_time, &_last_gps_origin_time_us, sizeof(uint64_t));
 	memcpy(origin_pos, &_pos_ref, sizeof(map_projection_reference_s));
@@ -726,7 +724,7 @@ bool Ekf::get_gps_drift_metrics(float drift[3], bool *blocked)
 }
 
 // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
-void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv)
+void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const
 {
 	// report absolute accuracy taking into account the uncertainty in location of the origin
 	// If not aiding, return 0 for horizontal position estimate as no estimate is available
@@ -748,7 +746,7 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv)
 }
 
 // get the 1-sigma horizontal and vertical position uncertainty of the ekf local position
-void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv)
+void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const
 {
 	// TODO - allow for baro drift in vertical position error
 	float hpos_err = sqrtf(P(7, 7) + P(8, 8));
@@ -765,7 +763,7 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv)
 }
 
 // get the 1-sigma horizontal and vertical velocity uncertainty
-void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv)
+void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
 {
 	float hvel_err = sqrtf(P(4, 4) + P(5, 5));
 
@@ -805,7 +803,7 @@ vz_max : Maximum ground relative vertical speed (meters/sec). NaN when limiting
 hagl_min : Minimum height above ground (meters). NaN when limiting is not needed.
 hagl_max : Maximum height above ground (meters). NaN when limiting is not needed.
 */
-void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max)
+void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const
 {
 	// Calculate range finder limits
 	const float rangefinder_hagl_min = _range_sensor.getValidMinVal();
@@ -845,14 +843,12 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
 		*hagl_min = fmaxf(rangefinder_hagl_min, flow_hagl_min);
 		*hagl_max = fminf(rangefinder_hagl_max, flow_hagl_max);
 	}
-
 }
 
 bool Ekf::reset_imu_bias()
 {
 	if (_imu_sample_delayed.time_us - _last_imu_bias_cov_reset_us < (uint64_t)10e6) {
 		return false;
-
 	}
 
 	// Zero the delta angle and delta velocity bias states
@@ -877,7 +873,7 @@ bool Ekf::reset_imu_bias()
 // A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF
 // Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned.
 void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas,
-				     float &hagl, float &beta)
+				     float &hagl, float &beta) const
 {
 	// return the integer bitmask containing the consistency check pass/fail status
 	status = _innov_check_fail_status.value;
@@ -900,7 +896,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 }
 
 // return a bitmask integer that describes which state estimates are valid
-void Ekf::get_ekf_soln_status(uint16_t *status)
+void Ekf::get_ekf_soln_status(uint16_t *status) const
 {
 	ekf_solution_status soln_status;
 	// TODO: Is this accurate enough?
@@ -941,13 +937,6 @@ void Ekf::uncorrelateQuatFromOtherStates()
 	P.slice<4, _k_num_states - 4>(0, 4) = 0.f;
 }
 
-bool Ekf::global_position_is_valid()
-{
-	// return true if the origin is set we are not doing unconstrained free inertial navigation
-	// and have not started using synthetic position observations to constrain drift
-	return (_NED_origin_initialised && !_deadreckon_time_exceeded && !_using_synthetic_position);
-}
-
 // return true if we are totally reliant on inertial dead-reckoning for position
 void Ekf::update_deadreckoning_status()
 {
@@ -1533,7 +1522,7 @@ bool Ekf::resetYawToEKFGSF()
 	// data and the yaw estimate has converged
 	float new_yaw, new_yaw_variance;
 
-	if (!yawEstimator.getYawData(&new_yaw, &new_yaw_variance)) {
+	if (!_yawEstimator.getYawData(&new_yaw, &new_yaw_variance)) {
 		return false;
 	}
 
@@ -1569,7 +1558,7 @@ bool Ekf::resetYawToEKFGSF()
 bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
 			float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF])
 {
-	return yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight);
+	return _yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight);
 }
 
 void Ekf::runYawEKFGSF()
@@ -1584,12 +1573,12 @@ void Ekf::runYawEKFGSF()
 	}
 
 	const Vector3f imu_gyro_bias = getGyroBias();
-	yawEstimator.update(_imu_sample_delayed, _control_status.flags.in_air, TAS, imu_gyro_bias);
+	_yawEstimator.update(_imu_sample_delayed, _control_status.flags.in_air, TAS, imu_gyro_bias);
 
 	// basic sanity check on GPS velocity data
 	if (_gps_data_ready && _gps_sample_delayed.vacc > FLT_EPSILON &&
 	    ISFINITE(_gps_sample_delayed.vel(0)) && ISFINITE(_gps_sample_delayed.vel(1))) {
-		yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), _gps_sample_delayed.vacc);
+		_yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), _gps_sample_delayed.vacc);
 	}
 }
 

EKF/estimator_interface.cpp

@@ -567,13 +567,6 @@ void EstimatorInterface::unallocate_buffers()
 	_output_vert_buffer.unallocate();
 	_drag_buffer.unallocate();
 	_auxvel_buffer.unallocate();
-
-}
-
-bool EstimatorInterface::local_position_is_valid()
-{
-	// return true if we are not doing unconstrained free inertial navigation
-	return !_deadreckon_time_exceeded;
 }
 
 bool EstimatorInterface::isOnlyActiveSourceOfHorizontalAiding(const bool aiding_flag) const
@@ -610,7 +603,6 @@ void EstimatorInterface::printBufferAllocationFailed(const char *buffer_name)
 void EstimatorInterface::print_status()
 {
 	ECL_INFO("local position valid: %s", local_position_is_valid() ? "yes" : "no");
-	ECL_INFO("global position valid: %s", global_position_is_valid() ? "yes" : "no");
 
 	ECL_INFO("imu buffer: %d (%d Bytes)", _imu_buffer.get_length(), _imu_buffer.get_total_size());
 	ECL_INFO("gps buffer: %d (%d Bytes)", _gps_buffer.get_length(), _gps_buffer.get_total_size());

EKF/estimator_interface.h

@@ -46,7 +46,6 @@
 #include "RingBuffer.h"
 #include <AlphaFilter/AlphaFilter.hpp>
 #include "imu_down_sampler.hpp"
-#include "EKFGSF_yaw.h"
 #include "sensor_range_finder.hpp"
 #include "utils.hpp"
 
@@ -58,106 +57,19 @@ using namespace estimator;
 
 class EstimatorInterface
 {
-
 public:
-	EstimatorInterface(): _imu_down_sampler(FILTER_UPDATE_PERIOD_S) {};
-	virtual ~EstimatorInterface() = default;
-
-	virtual bool init(uint64_t timestamp) = 0;
-	virtual void reset() = 0;
-
-	virtual bool update() = 0;
-
-	virtual void getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const = 0;
-	virtual void getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const = 0;
-	virtual void getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const = 0;
-
-	virtual void getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const = 0;
-	virtual void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const = 0;
-	virtual void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const = 0;
-
-	virtual void getBaroHgtInnov(float &baro_hgt_innov) const = 0;
-	virtual void getBaroHgtInnovVar(float &baro_hgt_innov_var) const = 0;
-	virtual void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const = 0;
-
-	virtual void getRngHgtInnov(float &rng_hgt_innov) const = 0;
-	virtual void getRngHgtInnovVar(float &rng_hgt_innov_var) const = 0;
-	virtual void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const = 0;
-
-	virtual void getAuxVelInnov(float aux_vel_innov[2]) const = 0;
-	virtual void getAuxVelInnovVar(float aux_vel_innov[2]) const = 0;
-	virtual void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const = 0;
-
-	virtual void getFlowInnov(float flow_innov[2]) const = 0;
-	virtual void getFlowInnovVar(float flow_innov_var[2]) const = 0;
-	virtual void getFlowInnovRatio(float &flow_innov_ratio) const = 0;
-	virtual const Vector2f &getFlowVelBody() const = 0;
-	virtual const Vector2f &getFlowVelNE() const = 0;
-	virtual const Vector2f &getFlowCompensated() const = 0;
-	virtual const Vector2f &getFlowUncompensated() const = 0;
-	virtual const Vector3f &getFlowGyro() const = 0;
-
-	virtual void getHeadingInnov(float &heading_innov) const = 0;
-	virtual void getHeadingInnovVar(float &heading_innov_var) const = 0;
-	virtual void getHeadingInnovRatio(float &heading_innov_ratio) const = 0;
-
-	virtual void getMagInnov(float mag_innov[3]) const = 0;
-	virtual void getMagInnovVar(float mag_innov_var[3]) const = 0;
-	virtual void getMagInnovRatio(float &mag_innov_ratio) const = 0;
-
-	virtual void getDragInnov(float drag_innov[2]) const = 0;
-	virtual void getDragInnovVar(float drag_innov_var[2]) const = 0;
-	virtual void getDragInnovRatio(float drag_innov_ratio[2]) const = 0;
-
-	virtual void getAirspeedInnov(float &airspeed_innov) const = 0;
-	virtual void getAirspeedInnovVar(float &get_airspeed_innov_var) const = 0;
-	virtual void getAirspeedInnovRatio(float &airspeed_innov_ratio) const = 0;
-
-	virtual void getBetaInnov(float &beta_innov) const = 0;
-	virtual void getBetaInnovVar(float &get_beta_innov_var) const = 0;
-	virtual void getBetaInnovRatio(float &beta_innov_ratio) const = 0;
-
-	virtual void getHaglInnov(float &hagl_innov) const = 0;
-	virtual void getHaglInnovVar(float &hagl_innov_var) const = 0;
-	virtual void getHaglInnovRatio(float &hagl_innov_ratio) const = 0;
-
-	virtual matrix::Vector<float, 24> getStateAtFusionHorizonAsVector() const = 0;
-
-	virtual Vector2f getWindVelocityVariance() const = 0;
-
-	virtual void get_true_airspeed(float *tas) = 0;
-
-	/*
-	First argument returns GPS drift  metrics in the following array locations
-	0 : Horizontal position drift rate (m/s)
-	1 : Vertical position drift rate (m/s)
-	2 : Filtered horizontal velocity (m/s)
-	Second argument returns true when IMU movement is blocking the drift calculation
-	Function returns true if the metrics have been updated and not returned previously by this function
-	*/
-	virtual bool get_gps_drift_metrics(float drift[3], bool *blocked) = 0;
-
-	// get the ekf WGS-84 origin position and height and the system time it was last set
-	// return true if the origin is valid
-	virtual bool get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) = 0;
-
-	// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
-	virtual void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) = 0;
-
-	// get the 1-sigma horizontal and vertical position uncertainty of the ekf local position
-	virtual void get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) = 0;
-
-	// get the 1-sigma horizontal and vertical velocity uncertainty
-	virtual void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) = 0;
-
-	// get the vehicle control limits required by the estimator to keep within sensor limitations
-	virtual void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) = 0;
-
 	// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
 	virtual bool collect_gps(const gps_message &gps) = 0;
 
 	void setIMUData(const imuSample &imu_sample);
 
+	/*
+	Returns  following IMU vibration metrics in the following array locations
+	0 : Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle)
+	1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle)
+	2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity)
+	*/
+	const Vector3f &getImuVibrationMetrics() const { return _vibe_metrics; }
 
 	void setMagData(const magSample &mag_sample);
 
@@ -182,29 +94,22 @@ public:
 	parameters *getParamHandle() { return &_params; }
 
 	// set vehicle landed status data
-	void set_in_air_status(bool in_air) {_control_status.flags.in_air = in_air;}
+	void set_in_air_status(bool in_air) { _control_status.flags.in_air = in_air; }
-
-	/*
-	Reset all IMU bias states and covariances to initial alignment values.
-	Use when the IMU sensor has changed.
-	Returns true if reset performed, false if rejected due to less than 10 seconds lapsed since last reset.
-	*/
-	virtual bool reset_imu_bias() = 0;
 
 	// return true if the attitude is usable
-	bool attitude_valid() { return ISFINITE(_output_new.quat_nominal(0)) && _control_status.flags.tilt_align; }
+	bool attitude_valid() const { return ISFINITE(_output_new.quat_nominal(0)) && _control_status.flags.tilt_align; }
 
 	// get vehicle landed status data
-	bool get_in_air_status() {return _control_status.flags.in_air;}
+	bool get_in_air_status() const { return _control_status.flags.in_air; }
 
 	// get wind estimation status
-	bool get_wind_status() { return _control_status.flags.wind; }
+	bool get_wind_status() const { return _control_status.flags.wind; }
 
 	// set vehicle is fixed wing status
-	void set_is_fixed_wing(bool is_fixed_wing) {_control_status.flags.fixed_wing = is_fixed_wing;}
+	void set_is_fixed_wing(bool is_fixed_wing) { _control_status.flags.fixed_wing = is_fixed_wing; }
 
 	// set flag if synthetic sideslip measurement should be fused
-	void set_fuse_beta_flag(bool fuse_beta) {_control_status.flags.fuse_beta = (fuse_beta && _control_status.flags.in_air);}
+	void set_fuse_beta_flag(bool fuse_beta) { _control_status.flags.fuse_beta = (fuse_beta && _control_status.flags.in_air); }
 
 	// set flag if static pressure rise due to ground effect is expected
 	// use _params.gnd_effect_deadzone to adjust for expected rise in static pressure
@@ -216,7 +121,7 @@ public:
 	}
 
 	// set air density used by the multi-rotor specific drag force fusion
-	void set_air_density(float air_density) {_air_density = air_density;}
+	void set_air_density(float air_density) { _air_density = air_density; }
 
 	// set sensor limitations reported by the rangefinder
 	void set_rangefinder_limits(float min_distance, float max_distance)
@@ -232,9 +137,6 @@ public:
 		_flow_max_distance = max_distance;
 	}
 
-	// return true if the global position estimate is valid
-	virtual bool global_position_is_valid() = 0;
-
 	// the flags considered are opt_flow, gps, ev_vel and ev_pos
 	bool isOnlyActiveSourceOfHorizontalAiding(bool aiding_flag) const;
 
@@ -256,29 +158,16 @@ public:
 
 	int getNumberOfActiveHorizontalAidingSources() const;
 
-	// return true if the EKF is dead reckoning the position using inertial data only
-	bool inertial_dead_reckoning() {return _is_dead_reckoning;}
-
-	virtual bool isTerrainEstimateValid() const = 0;
-	//[[deprecated("Replaced by isTerrainEstimateValid")]]
-	bool get_terrain_valid() { return isTerrainEstimateValid(); }
-
-	virtual uint8_t getTerrainEstimateSensorBitfield() const = 0;
-
-	// get the estimated terrain vertical position relative to the NED origin
-	virtual float getTerrainVertPos() const = 0;
-
 	// return true if the local position estimate is valid
-	bool local_position_is_valid();
+	bool local_position_is_valid() const { return !_deadreckon_time_exceeded; }
+
+	// return true if the EKF is dead reckoning the position using inertial data only
+	bool inertial_dead_reckoning() const { return _is_dead_reckoning; }
 
 	const matrix::Quatf &getQuaternion() const { return _output_new.quat_nominal; }
 
 	// get the velocity of the body frame origin in local NED earth frame
-	Vector3f getVelocity() const
+	Vector3f getVelocity() const { return _output_new.vel - _vel_imu_rel_body_ned; }
-	{
-		const Vector3f vel_earth = _output_new.vel - _vel_imu_rel_body_ned;
-		return vel_earth;
-	}
 
 	// get the velocity derivative in earth frame
 	const Vector3f &getVelocityDerivative() const { return _vel_deriv; }
@@ -298,7 +187,7 @@ public:
 	// Get the value of magnetic declination in degrees to be saved for use at the next startup
 	// Returns true when the declination can be saved
 	// At the next startup, set param.mag_declination_deg to the value saved
-	bool get_mag_decl_deg(float *val)
+	bool get_mag_decl_deg(float *val) const
 	{
 		if (_NED_origin_initialised && (_params.mag_declination_source & MASK_SAVE_GEO_DECL)) {
 			*val = math::degrees(_mag_declination_gps);
@@ -310,69 +199,35 @@ public:
 	}
 
 	// get EKF mode status
-	void get_control_mode(uint32_t *val) { *val = _control_status.value; }
+	void get_control_mode(uint32_t *val) const { *val = _control_status.value; }
 
 	// get EKF internal fault status
-	void get_filter_fault_status(uint16_t *val) { *val = _fault_status.value; }
+	void get_filter_fault_status(uint16_t *val) const { *val = _fault_status.value; }
 
 	bool isVehicleAtRest() const { return _control_status.flags.vehicle_at_rest; }
 
-	// get GPS check status
-	virtual void get_gps_check_status(uint16_t *val) = 0;
-
-	// return the amount the local vertical position changed in the last reset and the number of reset events
-	virtual void get_posD_reset(float *delta, uint8_t *counter) = 0;
-
-	// return the amount the local vertical velocity changed in the last reset and the number of reset events
-	virtual void get_velD_reset(float *delta, uint8_t *counter) = 0;
-
-	// return the amount the local horizontal position changed in the last reset and the number of reset events
-	virtual void get_posNE_reset(float delta[2], uint8_t *counter) = 0;
-
-	// return the amount the local horizontal velocity changed in the last reset and the number of reset events
-	virtual void get_velNE_reset(float delta[2], uint8_t *counter) = 0;
-
-	// return the amount the quaternion has changed in the last reset and the number of reset events
-	virtual void get_quat_reset(float delta_quat[4], uint8_t *counter) = 0;
-
-	// get EKF innovation consistency check status information comprising of:
-	// status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check
-	// Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold.
-	// A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF
-	// Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned.
-	virtual void get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas,
-						float &hagl, float &beta) = 0;
-
-	// return a bitmask integer that describes which state estimates can be used for flight control
-	virtual void get_ekf_soln_status(uint16_t *status) = 0;
-
 	// Getter for the average imu update period in s
 	float get_dt_imu_avg() const { return _dt_imu_avg; }
 
 	// Getter for the imu sample on the delayed time horizon
-	const imuSample &get_imu_sample_delayed() { return _imu_sample_delayed; }
+	const imuSample &get_imu_sample_delayed() const { return _imu_sample_delayed; }
 
 	// Getter for the baro sample on the delayed time horizon
-	const baroSample &get_baro_sample_delayed() { return _baro_sample_delayed; }
+	const baroSample &get_baro_sample_delayed() const { return _baro_sample_delayed; }
 
 	void print_status();
 
 	static constexpr unsigned FILTER_UPDATE_PERIOD_MS{10};	// ekf prediction period in milliseconds - this should ideally be an integer multiple of the IMU time delta
 	static constexpr float FILTER_UPDATE_PERIOD_S{FILTER_UPDATE_PERIOD_MS * 0.001f};
 
-	// request the EKF reset the yaw to the estimate from the internal EKF-GSF filter
-	// argment should be incremented only when a new reset is required
-	virtual void requestEmergencyNavReset() = 0;
-
-	// get ekf-gsf debug data
-	virtual bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
-				   float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) = 0;
-
 protected:
 
-	parameters _params;		// filter parameters
+	EstimatorInterface() = default;
+	virtual ~EstimatorInterface() = default;
 
-	ImuDownSampler _imu_down_sampler;
+	virtual bool init(uint64_t timestamp) = 0;
+
+	parameters _params;		// filter parameters
 
 	/*
 	 OBS_BUFFER_LENGTH defines how many observations (non-IMU measurements) we can buffer
@@ -391,8 +246,6 @@ protected:
 	*/
 	uint8_t _imu_buffer_length{0};
 
-	unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec)
-
 	float _dt_imu_avg{0.0f};	// average imu update period in s
 
 	imuSample _imu_sample_delayed{};	// captures the imu sample on the delayed time horizon
@@ -409,9 +262,6 @@ protected:
 	dragSample _drag_down_sampled{};	// down sampled drag specific force data (filter prediction rate -> observation rate)
 	auxVelSample _auxvel_sample_delayed{};
 
-	// Used by the multi-rotor specific drag force fusion
-	uint8_t _drag_sample_count{0};	// number of drag specific force samples assumulated at the filter prediction rate
-	float _drag_sample_time_dt{0.0f};	// time integral across all samples used to form _drag_down_sampled (sec)
 	float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C};		// air density (kg/m**3)
 
 	// Sensor limitations
@@ -460,13 +310,6 @@ protected:
 	bool _deadreckon_time_exceeded{true};	// true if the horizontal nav solution has been deadreckoning for too long and is invalid
 	bool _is_wind_dead_reckoning{false};	// true if we are navigationg reliant on wind relative measurements
 
-	// IMU vibration and movement monitoring
-	Vector3f _delta_ang_prev;	// delta angle from the previous IMU measurement
-	Vector3f _delta_vel_prev;	// delta velocity from the previous IMU measurement
-	Vector3f _vibe_metrics;	// IMU vibration metrics
-					// [0] Level of coning vibration in the IMU delta angles (rad^2)
-					// [1] high frequency vibration level in the IMU delta angle data (rad)
-					// [2] high frequency vibration level in the IMU delta velocity data (m/s)
 	float _gps_drift_metrics[3] {};	// Array containing GPS drift metrics
 					// [0] Horizontal position drift rate (m/s)
 					// [1] Vertical position drift rate (m/s)
@@ -488,20 +331,6 @@ protected:
 	RingBuffer<dragSample> _drag_buffer;
 	RingBuffer<auxVelSample> _auxvel_buffer;
 
-	// yaw estimator instance
-	EKFGSF_yaw yawEstimator;
-
-	// observation buffer final allocation failed
-	bool _gps_buffer_fail{false};
-	bool _mag_buffer_fail{false};
-	bool _baro_buffer_fail{false};
-	bool _range_buffer_fail{false};
-	bool _airspeed_buffer_fail{false};
-	bool _flow_buffer_fail{false};
-	bool _ev_buffer_fail{false};
-	bool _drag_buffer_fail{false};
-	bool _auxvel_buffer_fail{false};
-
 	// timestamps of latest in buffer saved measurement in microseconds
 	uint64_t _time_last_imu{0};
 	uint64_t _time_last_gps{0};
@@ -515,24 +344,11 @@ protected:
 	//last time the baro ground effect compensation was turned on externally (uSec)
 	uint64_t _time_last_gnd_effect_on{0};
 
-	// Used to downsample magnetometer data
-	Vector3f _mag_data_sum;
-	uint8_t _mag_sample_count {0};
-	uint64_t _mag_timestamp_sum {0};
-
-	// Used to down sample barometer data
-	float _baro_alt_sum {0.0f};			// summed pressure altitude readings (m)
-	uint8_t _baro_sample_count {0};		// number of barometric altitude measurements summed
-	uint64_t _baro_timestamp_sum {0};	// summed timestamp to provide the timestamp of the averaged sample
-
 	fault_status_u _fault_status{};
 
 	// allocate data buffers and initialize interface variables
 	bool initialise_interface(uint64_t timestamp);
 
-	// free buffer memory
-	void unallocate_buffers();
-
 	float _mag_declination_gps{NAN};         // magnetic declination returned by the geo library using the last valid GPS position (rad)
 	float _mag_inclination_gps{NAN};	  // magnetic inclination returned by the geo library using the last valid GPS position (rad)
 	float _mag_strength_gps{NAN};	          // magnetic strength returned by the geo library using the last valid GPS position (T)
@@ -543,12 +359,55 @@ protected:
 	// this is the previous status of the filter control modes - used to detect mode transitions
 	filter_control_status_u _control_status_prev{};
 
+	virtual float compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const = 0;
+
+private:
+
 	inline void setDragData(const imuSample &imu);
 
 	inline void computeVibrationMetric(const imuSample &imu);
 	inline bool checkIfVehicleAtRest(float dt, const imuSample &imu);
 
-	virtual float compensateBaroForDynamicPressure(const float baro_alt_uncompensated) = 0;
-
 	void printBufferAllocationFailed(const char *buffer_name);
+
+	// free buffer memory
+	void unallocate_buffers();
+
+	ImuDownSampler _imu_down_sampler{FILTER_UPDATE_PERIOD_S};
+
+	unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec)
+
+	// IMU vibration and movement monitoring
+	Vector3f _delta_ang_prev;	// delta angle from the previous IMU measurement
+	Vector3f _delta_vel_prev;	// delta velocity from the previous IMU measurement
+	Vector3f _vibe_metrics;	// IMU vibration metrics
+					// [0] Level of coning vibration in the IMU delta angles (rad^2)
+					// [1] high frequency vibration level in the IMU delta angle data (rad)
+					// [2] high frequency vibration level in the IMU delta velocity data (m/s)
+
+	// Used to down sample barometer data
+	uint64_t _baro_timestamp_sum{0};	// summed timestamp to provide the timestamp of the averaged sample
+	float _baro_alt_sum{0.0f};			// summed pressure altitude readings (m)
+	uint8_t _baro_sample_count{0};		// number of barometric altitude measurements summed
+
+	// Used by the multi-rotor specific drag force fusion
+	uint8_t _drag_sample_count{0};	// number of drag specific force samples assumulated at the filter prediction rate
+	float _drag_sample_time_dt{0.0f};	// time integral across all samples used to form _drag_down_sampled (sec)
+
+	// Used to downsample magnetometer data
+	uint64_t _mag_timestamp_sum{0};
+	Vector3f _mag_data_sum;
+	uint8_t _mag_sample_count{0};
+
+	// observation buffer final allocation failed
+	bool _gps_buffer_fail{false};
+	bool _mag_buffer_fail{false};
+	bool _baro_buffer_fail{false};
+	bool _range_buffer_fail{false};
+	bool _airspeed_buffer_fail{false};
+	bool _flow_buffer_fail{false};
+	bool _ev_buffer_fail{false};
+	bool _drag_buffer_fail{false};
+	bool _auxvel_buffer_fail{false};
+
 };

EKF/mag_control.cpp

@@ -138,11 +138,6 @@ void Ekf::checkHaglYawResetReq()
 	}
 }
 
-float Ekf::getTerrainVPos() const
-{
-	return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD;
-}
-
 void Ekf::runOnGroundYawReset()
 {
 	if (_mag_yaw_reset_req && isYawResetAuthorized()) {
@@ -154,11 +149,6 @@ void Ekf::runOnGroundYawReset()
 	}
 }
 
-bool Ekf::isYawResetAuthorized() const
-{
-	return !_is_yaw_fusion_inhibited;
-}
-
 bool Ekf::canResetMagHeading() const
 {
 	return !isStrongMagneticDisturbance() && (_params.mag_fusion_type != MAG_FUSE_TYPE_NONE);
@@ -177,11 +167,6 @@ void Ekf::runInAirYawReset()
 	}
 }
 
-bool Ekf::canRealignYawUsingGps() const
-{
-	return _control_status.flags.fixed_wing;
-}
-
 void Ekf::runVelPosReset()
 {
 	if (_velpos_reset_request) {
@@ -237,16 +222,6 @@ void Ekf::checkMagBiasObservability()
 	_time_yaw_started = _imu_sample_delayed.time_us;
 }
 
-bool Ekf::isYawAngleObservable() const
-{
-	return _yaw_angle_observable;
-}
-
-bool Ekf::isMagBiasObservable() const
-{
-	return _mag_bias_observable;
-}
-
 bool Ekf::canUse3DMagFusion() const
 {
 	// Use of 3D fusion requires an in-air heading alignment but it should not
@@ -308,11 +283,6 @@ void Ekf::checkMagFieldStrength()
 	}
 }
 
-bool Ekf::isStrongMagneticDisturbance() const
-{
-	return _control_status.flags.mag_field_disturbed;
-}
-
 bool Ekf::isMeasuredMatchingGpsMagStrength() const
 {
 	constexpr float wmm_gate_size = 0.2f; // +/- Gauss

EKF/terrain_estimator.cpp

@@ -83,16 +83,6 @@ bool Ekf::initHagl()
 	return initialized;
 }
 
-bool Ekf::shouldUseRangeFinderForHagl() const
-{
-	return (_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder);
-}
-
-bool Ekf::shouldUseOpticalFlowForHagl() const
-{
-	return (_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseOpticalFlow);
-}
-
 void Ekf::runTerrainEstimator()
 {
 	// If we are on ground, store the local position and time to use as a reference
@@ -286,11 +276,6 @@ void Ekf::fuseFlowForTerrain()
 	}
 }
 
-bool Ekf::isTerrainEstimateValid() const
-{
-	return _hagl_valid;
-}
-
 void Ekf::updateTerrainValidity()
 {
 	// we have been fusing range finder measurements in the last 5 seconds
@@ -306,12 +291,6 @@ void Ekf::updateTerrainValidity()
 	_hagl_sensor_status.flags.range_finder = shouldUseRangeFinderForHagl()
 						 && recent_range_fusion
 						 && (_time_last_fake_hagl_fuse != _time_last_hagl_fuse);
-	_hagl_sensor_status.flags.flow = shouldUseOpticalFlowForHagl()
-					 && recent_flow_for_terrain_fusion;
-}
 
-// get the estimated vertical position of the terrain relative to the NED origin
+	_hagl_sensor_status.flags.flow = shouldUseOpticalFlowForHagl() && recent_flow_for_terrain_fusion;
-float Ekf::getTerrainVertPos() const
-{
-	return _terrain_vpos;
 }