ekf2: delete redundant aid src status getters

msg/EstimatorInnovations.msg

@@ -19,7 +19,6 @@ float32 baro_vpos	# barometer height innovation (m) and innovation variance (m**
 
 # Auxiliary velocity
 float32[2] aux_hvel	# horizontal auxiliary velocity innovation from landing target measurement (m/sec) and innovation variance ((m/sec)**2)
-float32    aux_vvel	# vertical auxiliary velocity innovation from landing target measurement (m/sec) and innovation variance ((m/sec)**2)
 
 # Optical flow
 float32[2] flow		# flow innvoation (rad/sec) and innovation variance ((rad/sec)**2)

src/modules/ekf2/EKF/ekf.h

@@ -83,19 +83,9 @@ public:
 
 	static uint8_t getNumberOfStates() { return State::size; }
 
-#if defined(CONFIG_EKF2_EXTERNAL_VISION)
-	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;
-	void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const;
-#endif // CONFIG_EKF2_EXTERNAL_VISION
-
 #if defined(CONFIG_EKF2_BAROMETER)
 	const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; }
 	const BiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); }
-
-	void getBaroHgtInnov(float &baro_hgt_innov) const { baro_hgt_innov = _aid_src_baro_hgt.innovation; }
-	void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _aid_src_baro_hgt.innovation_variance; }
-	void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const { baro_hgt_innov_ratio = _aid_src_baro_hgt.test_ratio; }
 #endif // CONFIG_EKF2_BAROMETER
 
 #if defined(CONFIG_EKF2_TERRAIN)
@@ -115,28 +105,10 @@ public:
 
 # if defined(CONFIG_EKF2_RANGE_FINDER)
 	const auto &aid_src_terrain_range_finder() const { return _aid_src_terrain_range_finder; }
-
-	void getHaglInnov(float &hagl_innov) const { hagl_innov = _aid_src_terrain_range_finder.innovation; }
-	void getHaglInnovVar(float &hagl_innov_var) const { hagl_innov_var = _aid_src_terrain_range_finder.innovation_variance; }
-	void getHaglInnovRatio(float &hagl_innov_ratio) const { hagl_innov_ratio = _aid_src_terrain_range_finder.test_ratio; }
 # endif // CONFIG_EKF2_RANGE_FINDER
 
 # if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	const auto &aid_src_terrain_optical_flow() const { return _aid_src_terrain_optical_flow; }
-
-	void getTerrainFlowInnov(float flow_innov[2]) const
-	{
-		flow_innov[0] = _aid_src_terrain_optical_flow.innovation[0];
-		flow_innov[1] = _aid_src_terrain_optical_flow.innovation[1];
-	}
-
-	void getTerrainFlowInnovVar(float flow_innov_var[2]) const
-	{
-		flow_innov_var[0] = _aid_src_terrain_optical_flow.innovation_variance[0];
-		flow_innov_var[1] = _aid_src_terrain_optical_flow.innovation_variance[1];
-	}
-
-	void getTerrainFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_terrain_optical_flow.test_ratio[0], _aid_src_terrain_optical_flow.test_ratio[1]); }
 # endif // CONFIG_EKF2_OPTICAL_FLOW
 
 #endif // CONFIG_EKF2_TERRAIN
@@ -146,32 +118,14 @@ public:
 	const BiasEstimator::status &getRngHgtBiasEstimatorStatus() const { return _rng_hgt_b_est.getStatus(); }
 	const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; }
 
-	void getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _aid_src_rng_hgt.innovation; }
+	float getHaglRateInnov() const { return _rng_consistency_check.getInnov(); }
-	void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _aid_src_rng_hgt.innovation_variance; }
+	float getHaglRateInnovVar() const { return _rng_consistency_check.getInnovVar(); }
-	void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _aid_src_rng_hgt.test_ratio; }
+	float getHaglRateInnovRatio() const { return _rng_consistency_check.getSignedTestRatioLpf(); }
-
-	void getHaglRateInnov(float &hagl_rate_innov) const { hagl_rate_innov = _rng_consistency_check.getInnov(); }
-	void getHaglRateInnovVar(float &hagl_rate_innov_var) const { hagl_rate_innov_var = _rng_consistency_check.getInnovVar(); }
-	void getHaglRateInnovRatio(float &hagl_rate_innov_ratio) const { hagl_rate_innov_ratio = _rng_consistency_check.getSignedTestRatioLpf(); }
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; }
 
-	void getFlowInnov(float flow_innov[2]) const
-	{
-		flow_innov[0] = _aid_src_optical_flow.innovation[0];
-		flow_innov[1] = _aid_src_optical_flow.innovation[1];
-	}
-
-	void getFlowInnovVar(float flow_innov_var[2]) const
-	{
-		flow_innov_var[0] = _aid_src_optical_flow.innovation_variance[0];
-		flow_innov_var[1] = _aid_src_optical_flow.innovation_variance[1];
-	}
-
-	void getFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_optical_flow.test_ratio[0], _aid_src_optical_flow.test_ratio[1]); }
-
 	const Vector2f &getFlowVelBody() const { return _flow_vel_body; }
 	const Vector2f &getFlowVelNE() const { return _flow_vel_ne; }
 
@@ -185,140 +139,93 @@ public:
 	const Vector3f &getMeasuredBodyRate() const { return _measured_body_rate; }
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
-#if defined(CONFIG_EKF2_AUXVEL)
+	float getHeadingInnov() const
-	void getAuxVelInnov(float aux_vel_innov[2]) const;
-	void getAuxVelInnovVar(float aux_vel_innov[2]) const;
-	void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const { aux_vel_innov_ratio = math::max(_aid_src_aux_vel.test_ratio[0], _aid_src_aux_vel.test_ratio[1]); }
-#endif // CONFIG_EKF2_AUXVEL
-
-	void getHeadingInnov(float &heading_innov) const
 	{
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 		if (_control_status.flags.mag_hdg) {
-			heading_innov = _aid_src_mag_heading.innovation;
+			return _aid_src_mag_heading.innovation;
-			return;
+		}
 
-		} else if (_control_status.flags.mag_3D) {
+		if (_control_status.flags.mag_3D) {
-			heading_innov = Vector3f(_aid_src_mag.innovation).max();
+			return Vector3f(_aid_src_mag.innovation).max();
-			return;
 		}
 #endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_GNSS_YAW)
 		if (_control_status.flags.gps_yaw) {
-			heading_innov = _aid_src_gnss_yaw.innovation;
+			return _aid_src_gnss_yaw.innovation;
-			return;
 		}
 #endif // CONFIG_EKF2_GNSS_YAW
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 		if (_control_status.flags.ev_yaw) {
-			heading_innov = _aid_src_ev_yaw.innovation;
+			return _aid_src_ev_yaw.innovation;
-			return;
 		}
 #endif // CONFIG_EKF2_EXTERNAL_VISION
+
+		return 0.f;
 	}
 
-	void getHeadingInnovVar(float &heading_innov_var) const
+	float getHeadingInnovVar() const
 	{
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 		if (_control_status.flags.mag_hdg) {
-			heading_innov_var = _aid_src_mag_heading.innovation_variance;
+			return _aid_src_mag_heading.innovation_variance;
-			return;
+		}
 
-		} else if (_control_status.flags.mag_3D) {
+		if (_control_status.flags.mag_3D) {
-			heading_innov_var = Vector3f(_aid_src_mag.innovation_variance).max();
+			return Vector3f(_aid_src_mag.innovation_variance).max();
-			return;
 		}
 #endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_GNSS_YAW)
 		if (_control_status.flags.gps_yaw) {
-			heading_innov_var = _aid_src_gnss_yaw.innovation_variance;
+			return _aid_src_gnss_yaw.innovation_variance;
-			return;
 		}
 #endif // CONFIG_EKF2_GNSS_YAW
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 		if (_control_status.flags.ev_yaw) {
-			heading_innov_var = _aid_src_ev_yaw.innovation_variance;
+			return _aid_src_ev_yaw.innovation_variance;
-			return;
 		}
 #endif // CONFIG_EKF2_EXTERNAL_VISION
+
+		return 0.f;
 	}
 
-	void getHeadingInnovRatio(float &heading_innov_ratio) const
+	float getHeadingInnovRatio() const
 	{
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 		if (_control_status.flags.mag_hdg) {
-			heading_innov_ratio = _aid_src_mag_heading.test_ratio;
+			return _aid_src_mag_heading.test_ratio;
-			return;
+		}
 
-		} else if (_control_status.flags.mag_3D) {
+		if (_control_status.flags.mag_3D) {
-			heading_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max();
+			return Vector3f(_aid_src_mag.test_ratio).max();
-			return;
 		}
 #endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_GNSS_YAW)
 		if (_control_status.flags.gps_yaw) {
-			heading_innov_ratio = _aid_src_gnss_yaw.test_ratio;
+			return _aid_src_gnss_yaw.test_ratio;
-			return;
 		}
 #endif // CONFIG_EKF2_GNSS_YAW
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 		if (_control_status.flags.ev_yaw) {
-			heading_innov_ratio = _aid_src_ev_yaw.test_ratio;
+			return _aid_src_ev_yaw.test_ratio;
-			return;
 		}
 #endif // CONFIG_EKF2_EXTERNAL_VISION
-	}
 
-#if defined(CONFIG_EKF2_MAGNETOMETER)
+		return 0.f;
-	void getMagInnov(float mag_innov[3]) const { memcpy(mag_innov, _aid_src_mag.innovation, sizeof(_aid_src_mag.innovation)); }
+	}
-	void getMagInnovVar(float mag_innov_var[3]) const { memcpy(mag_innov_var, _aid_src_mag.innovation_variance, sizeof(_aid_src_mag.innovation_variance)); }
-	void getMagInnovRatio(float &mag_innov_ratio) const { mag_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max(); }
-#endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_DRAG_FUSION)
 	const auto &aid_src_drag() const { return _aid_src_drag; }
-
-	void getDragInnov(float drag_innov[2]) const
-	{
-		drag_innov[0] = _aid_src_drag.innovation[0];
-		drag_innov[1] = _aid_src_drag.innovation[1];
-	}
-	void getDragInnovVar(float drag_innov_var[2]) const
-	{
-		drag_innov_var[0] = _aid_src_drag.innovation_variance[0];
-		drag_innov_var[1] = _aid_src_drag.innovation_variance[1];
-	}
-	void getDragInnovRatio(float drag_innov_ratio[2]) const
-	{
-		drag_innov_ratio[0] = _aid_src_drag.test_ratio[0];
-		drag_innov_ratio[1] = _aid_src_drag.test_ratio[1];
-	}
 #endif // CONFIG_EKF2_DRAG_FUSION
 
-#if defined(CONFIG_EKF2_AIRSPEED)
-	void getAirspeedInnov(float &airspeed_innov) const { airspeed_innov = _aid_src_airspeed.innovation; }
-	void getAirspeedInnovVar(float &airspeed_innov_var) const { airspeed_innov_var = _aid_src_airspeed.innovation_variance; }
-	void getAirspeedInnovRatio(float &airspeed_innov_ratio) const { airspeed_innov_ratio = _aid_src_airspeed.test_ratio; }
-#endif // CONFIG_EKF2_AIRSPEED
-
-#if defined(CONFIG_EKF2_SIDESLIP)
-	void getBetaInnov(float &beta_innov) const { beta_innov = _aid_src_sideslip.innovation; }
-	void getBetaInnovVar(float &beta_innov_var) const { beta_innov_var = _aid_src_sideslip.innovation_variance; }
-	void getBetaInnovRatio(float &beta_innov_ratio) const { beta_innov_ratio = _aid_src_sideslip.test_ratio; }
-#endif // CONFIG_EKF2_SIDESLIP
-
 #if defined(CONFIG_EKF2_GRAVITY_FUSION)
 	const auto &aid_src_gravity() const { return _aid_src_gravity; }
-
-	void getGravityInnov(float grav_innov[3]) const { memcpy(grav_innov, _aid_src_gravity.innovation, sizeof(_aid_src_gravity.innovation)); }
-	void getGravityInnovVar(float grav_innov_var[3]) const { memcpy(grav_innov_var, _aid_src_gravity.innovation_variance, sizeof(_aid_src_gravity.innovation_variance)); }
-	void getGravityInnovRatio(float &grav_innov_ratio) const { grav_innov_ratio = Vector3f(_aid_src_gravity.test_ratio).max(); }
 #endif // CONFIG_EKF2_GRAVITY_FUSION
 
 	// get the state vector at the delayed time horizon
@@ -497,12 +404,6 @@ public:
 
 	uint8_t getHeightSensorRef() const { return _height_sensor_ref; }
 
-#if defined(CONFIG_EKF2_EXTERNAL_VISION)
-	const BiasEstimator::status &getEvHgtBiasEstimatorStatus() const { return _ev_hgt_b_est.getStatus(); }
-
-	const BiasEstimator::status &getEvPosBiasEstimatorStatus(int i) const { return _ev_pos_b_est.getStatus(i); }
-#endif // CONFIG_EKF2_EXTERNAL_VISION
-
 #if defined(CONFIG_EKF2_AIRSPEED)
 	const auto &aid_src_airspeed() const { return _aid_src_airspeed; }
 #endif // CONFIG_EKF2_AIRSPEED
@@ -519,13 +420,12 @@ public:
 	const auto &aid_src_ev_pos() const { return _aid_src_ev_pos; }
 	const auto &aid_src_ev_vel() const { return _aid_src_ev_vel; }
 	const auto &aid_src_ev_yaw() const { return _aid_src_ev_yaw; }
+
+	const BiasEstimator::status &getEvHgtBiasEstimatorStatus() const { return _ev_hgt_b_est.getStatus(); }
+	const BiasEstimator::status &getEvPosBiasEstimatorStatus(int i) const { return _ev_pos_b_est.getStatus(i); }
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
 #if defined(CONFIG_EKF2_GNSS)
-	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;
-	void getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const;
-
 	// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
 	bool collect_gps(const gpsMessage &gps) override;
 

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -295,86 +295,6 @@ Vector3f Ekf::calcEarthRateNED(float lat_rad) const
 			-CONSTANTS_EARTH_SPIN_RATE * sinf(lat_rad));
 }
 
-#if defined(CONFIG_EKF2_GNSS)
-void Ekf::getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2],  float &vpos) const
-{
-	hvel[0] = _aid_src_gnss_vel.innovation[0];
-	hvel[1] = _aid_src_gnss_vel.innovation[1];
-	vvel    = _aid_src_gnss_vel.innovation[2];
-
-	hpos[0] = _aid_src_gnss_pos.innovation[0];
-	hpos[1] = _aid_src_gnss_pos.innovation[1];
-	vpos    = _aid_src_gnss_hgt.innovation;
-}
-
-void Ekf::getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos)  const
-{
-	hvel[0] = _aid_src_gnss_vel.innovation_variance[0];
-	hvel[1] = _aid_src_gnss_vel.innovation_variance[1];
-	vvel    = _aid_src_gnss_vel.innovation_variance[2];
-
-	hpos[0] = _aid_src_gnss_pos.innovation_variance[0];
-	hpos[1] = _aid_src_gnss_pos.innovation_variance[1];
-	vpos    = _aid_src_gnss_hgt.innovation_variance;
-}
-
-void Ekf::getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const
-{
-	hvel = fmaxf(_aid_src_gnss_vel.test_ratio[0], _aid_src_gnss_vel.test_ratio[1]);
-	vvel = _aid_src_gnss_vel.test_ratio[2];
-
-	hpos = fmaxf(_aid_src_gnss_pos.test_ratio[0], _aid_src_gnss_pos.test_ratio[1]);
-	vpos = _aid_src_gnss_hgt.test_ratio;
-}
-#endif // CONFIG_EKF2_GNSS
-
-#if defined(CONFIG_EKF2_EXTERNAL_VISION)
-void Ekf::getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const
-{
-	hvel[0] = _aid_src_ev_vel.innovation[0];
-	hvel[1] = _aid_src_ev_vel.innovation[1];
-	vvel    = _aid_src_ev_vel.innovation[2];
-
-	hpos[0] = _aid_src_ev_pos.innovation[0];
-	hpos[1] = _aid_src_ev_pos.innovation[1];
-	vpos    = _aid_src_ev_hgt.innovation;
-}
-
-void Ekf::getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const
-{
-	hvel[0] = _aid_src_ev_vel.innovation_variance[0];
-	hvel[1] = _aid_src_ev_vel.innovation_variance[1];
-	vvel    = _aid_src_ev_vel.innovation_variance[2];
-
-	hpos[0] = _aid_src_ev_pos.innovation_variance[0];
-	hpos[1] = _aid_src_ev_pos.innovation_variance[1];
-	vpos    = _aid_src_ev_hgt.innovation_variance;
-}
-
-void Ekf::getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const
-{
-	hvel = fmaxf(_aid_src_ev_vel.test_ratio[0], _aid_src_ev_vel.test_ratio[1]);
-	vvel = _aid_src_ev_vel.test_ratio[2];
-
-	hpos = fmaxf(_aid_src_ev_pos.test_ratio[0], _aid_src_ev_pos.test_ratio[1]);
-	vpos = _aid_src_ev_hgt.test_ratio;
-}
-#endif // CONFIG_EKF2_EXTERNAL_VISION
-
-#if defined(CONFIG_EKF2_AUXVEL)
-void Ekf::getAuxVelInnov(float aux_vel_innov[2]) const
-{
-	aux_vel_innov[0] = _aid_src_aux_vel.innovation[0];
-	aux_vel_innov[1] = _aid_src_aux_vel.innovation[1];
-}
-
-void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const
-{
-	aux_vel_innov_var[0] = _aid_src_aux_vel.innovation_variance[0];
-	aux_vel_innov_var[1] = _aid_src_aux_vel.innovation_variance[1];
-}
-#endif // CONFIG_EKF2_AUXVEL
-
 bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const
 {
 	origin_time = _pos_ref.getProjectionReferenceTimestamp();

src/modules/ekf2/EKF2.cpp

@@ -1406,53 +1406,93 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
 	// publish estimator innovation data
 	estimator_innovations_s innovations{};
 	innovations.timestamp_sample = _ekf.time_delayed_us();
+
 #if defined(CONFIG_EKF2_GNSS)
-	_ekf.getGpsVelPosInnov(innovations.gps_hvel, innovations.gps_vvel, innovations.gps_hpos, innovations.gps_vpos);
+	// GPS
+	innovations.gps_hvel[0] = _ekf.aid_src_gnss_vel().innovation[0];
+	innovations.gps_hvel[1] = _ekf.aid_src_gnss_vel().innovation[1];
+	innovations.gps_vvel    = _ekf.aid_src_gnss_vel().innovation[2];
+	innovations.gps_hpos[0] = _ekf.aid_src_gnss_pos().innovation[0];
+	innovations.gps_hpos[1] = _ekf.aid_src_gnss_pos().innovation[1];
+	innovations.gps_vpos    = _ekf.aid_src_gnss_hgt().innovation;
 #endif // CONFIG_EKF2_GNSS
+
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
-	_ekf.getEvVelPosInnov(innovations.ev_hvel, innovations.ev_vvel, innovations.ev_hpos, innovations.ev_vpos);
+	// External Vision
+	innovations.ev_hvel[0] = _ekf.aid_src_ev_vel().innovation[0];
+	innovations.ev_hvel[1] = _ekf.aid_src_ev_vel().innovation[1];
+	innovations.ev_vvel    = _ekf.aid_src_ev_vel().innovation[2];
+	innovations.ev_hpos[0] = _ekf.aid_src_ev_pos().innovation[0];
+	innovations.ev_hpos[1] = _ekf.aid_src_ev_pos().innovation[1];
+	innovations.ev_vpos    = _ekf.aid_src_ev_hgt().innovation;
 #endif // CONFIG_EKF2_EXTERNAL_VISION
-#if defined(CONFIG_EKF2_BAROMETER)
+
-	_ekf.getBaroHgtInnov(innovations.baro_vpos);
+	// Height sensors
-#endif // CONFIG_EKF2_BAROMETER
 #if defined(CONFIG_EKF2_RANGE_FINDER)
-	_ekf.getRngHgtInnov(innovations.rng_vpos);
+	innovations.rng_vpos = _ekf.aid_src_rng_hgt().innovation;
 #endif // CONFIG_EKF2_RANGE_FINDER
+#if defined(CONFIG_EKF2_BAROMETER)
+	innovations.baro_vpos = _ekf.aid_src_baro_hgt().innovation;
+#endif // CONFIG_EKF2_BAROMETER
+
 #if defined(CONFIG_EKF2_AUXVEL)
-	_ekf.getAuxVelInnov(innovations.aux_hvel);
+	// Auxiliary velocity
+	innovations.aux_hvel[0] = _ekf.aid_src_aux_vel().innovation[0];
+	innovations.aux_hvel[1] = _ekf.aid_src_aux_vel().innovation[1];
 #endif // CONFIG_EKF2_AUXVEL
+
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	_ekf.getFlowInnov(innovations.flow);
+	// Optical flow
+	innovations.flow[0] = _ekf.aid_src_optical_flow().innovation[0];
+	innovations.flow[1] = _ekf.aid_src_optical_flow().innovation[1];
+# if defined(CONFIG_EKF2_TERRAIN)
+	innovations.terr_flow[0] = _ekf.aid_src_terrain_optical_flow().innovation[0];
+	innovations.terr_flow[1] = _ekf.aid_src_terrain_optical_flow().innovation[1];
+# endif // CONFIG_EKF2_TERRAIN
 #endif // CONFIG_EKF2_OPTICAL_FLOW
-	_ekf.getHeadingInnov(innovations.heading);
+
+	// heading
+	innovations.heading = _ekf.getHeadingInnov();
+
 #if defined(CONFIG_EKF2_MAGNETOMETER)
-	_ekf.getMagInnov(innovations.mag_field);
+	// mag_field
+	innovations.mag_field[0] = _ekf.aid_src_mag().innovation[0];
+	innovations.mag_field[1] = _ekf.aid_src_mag().innovation[1];
+	innovations.mag_field[2] = _ekf.aid_src_mag().innovation[2];
 #endif // CONFIG_EKF2_MAGNETOMETER
-#if defined(CONFIG_EKF2_DRAG_FUSION)
-	_ekf.getDragInnov(innovations.drag);
-#endif // CONFIG_EKF2_DRAG_FUSION
-#if defined(CONFIG_EKF2_AIRSPEED)
-	_ekf.getAirspeedInnov(innovations.airspeed);
-#endif // CONFIG_EKF2_AIRSPEED
-#if defined(CONFIG_EKF2_SIDESLIP)
-	_ekf.getBetaInnov(innovations.beta);
-#endif // CONFIG_EKF2_SIDESLIP
 
 #if defined(CONFIG_EKF2_GRAVITY_FUSION)
-	_ekf.getGravityInnov(innovations.gravity);
+	// gravity
+	innovations.gravity[0] = _ekf.aid_src_gravity().innovation[0];
+	innovations.gravity[1] = _ekf.aid_src_gravity().innovation[1];
+	innovations.gravity[2] = _ekf.aid_src_gravity().innovation[2];
 #endif // CONFIG_EKF2_GRAVITY_FUSION
 
-#if defined(CONFIG_EKF2_TERRAIN)
+#if defined(CONFIG_EKF2_DRAG_FUSION)
-# if defined(CONFIG_EKF2_RANGE_FINDER)
+	// drag
-	_ekf.getHaglInnov(innovations.hagl);
+	innovations.drag[0] = _ekf.aid_src_drag().innovation[0];
-# endif //CONFIG_EKF2_RANGE_FINDER
+	innovations.drag[1] = _ekf.aid_src_drag().innovation[1];
-# if defined(CONFIG_EKF2_OPTICAL_FLOW)
+#endif // CONFIG_EKF2_DRAG_FUSION
-	_ekf.getTerrainFlowInnov(innovations.terr_flow);
-# endif // CONFIG_EKF2_OPTICAL_FLOW
-#endif // CONFIG_EKF2_TERRAIN
 
-	// Not yet supported
+#if defined(CONFIG_EKF2_AIRSPEED)
-	innovations.aux_vvel = NAN;
+	// airspeed
+	innovations.airspeed = _ekf.aid_src_airspeed().innovation;
+#endif // CONFIG_EKF2_AIRSPEED
+
+#if defined(CONFIG_EKF2_SIDESLIP)
+	// beta
+	innovations.beta = _ekf.aid_src_sideslip().innovation;
+#endif // CONFIG_EKF2_SIDESLIP
+
+#if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_RANGE_FINDER)
+	// hagl
+	innovations.hagl = _ekf.aid_src_terrain_range_finder().innovation;
+#endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER
+
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+	// hagl_rate
+	innovations.hagl_rate = _ekf.getHaglRateInnov();
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 	innovations.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 	_estimator_innovations_pub.publish(innovations);
@@ -1500,55 +1540,93 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
 	// publish estimator innovation test ratio data
 	estimator_innovations_s test_ratios{};
 	test_ratios.timestamp_sample = _ekf.time_delayed_us();
+
 #if defined(CONFIG_EKF2_GNSS)
-	_ekf.getGpsVelPosInnovRatio(test_ratios.gps_hvel[0], test_ratios.gps_vvel, test_ratios.gps_hpos[0],
+	// GPS
-				    test_ratios.gps_vpos);
+	test_ratios.gps_hvel[0] = _ekf.aid_src_gnss_vel().test_ratio[0];
+	test_ratios.gps_hvel[1] = _ekf.aid_src_gnss_vel().test_ratio[1];
+	test_ratios.gps_vvel    = _ekf.aid_src_gnss_vel().test_ratio[2];
+	test_ratios.gps_hpos[0] = _ekf.aid_src_gnss_pos().test_ratio[0];
+	test_ratios.gps_hpos[1] = _ekf.aid_src_gnss_pos().test_ratio[1];
+	test_ratios.gps_vpos    = _ekf.aid_src_gnss_hgt().test_ratio;
 #endif // CONFIG_EKF2_GNSS
+
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
-	_ekf.getEvVelPosInnovRatio(test_ratios.ev_hvel[0], test_ratios.ev_vvel, test_ratios.ev_hpos[0], test_ratios.ev_vpos);
+	// External Vision
+	test_ratios.ev_hvel[0] = _ekf.aid_src_ev_vel().test_ratio[0];
+	test_ratios.ev_hvel[1] = _ekf.aid_src_ev_vel().test_ratio[1];
+	test_ratios.ev_vvel    = _ekf.aid_src_ev_vel().test_ratio[2];
+	test_ratios.ev_hpos[0] = _ekf.aid_src_ev_pos().test_ratio[0];
+	test_ratios.ev_hpos[1] = _ekf.aid_src_ev_pos().test_ratio[1];
+	test_ratios.ev_vpos    = _ekf.aid_src_ev_hgt().test_ratio;
 #endif // CONFIG_EKF2_EXTERNAL_VISION
-#if defined(CONFIG_EKF2_BAROMETER)
+
-	_ekf.getBaroHgtInnovRatio(test_ratios.baro_vpos);
+	// Height sensors
-#endif // CONFIG_EKF2_BAROMETER
 #if defined(CONFIG_EKF2_RANGE_FINDER)
-	_ekf.getRngHgtInnovRatio(test_ratios.rng_vpos);
+	test_ratios.rng_vpos = _ekf.aid_src_rng_hgt().test_ratio;
-	_ekf.getHaglRateInnovRatio(test_ratios.hagl_rate);
 #endif // CONFIG_EKF2_RANGE_FINDER
+#if defined(CONFIG_EKF2_BAROMETER)
+	test_ratios.baro_vpos = _ekf.aid_src_baro_hgt().test_ratio;
+#endif // CONFIG_EKF2_BAROMETER
+
 #if defined(CONFIG_EKF2_AUXVEL)
-	_ekf.getAuxVelInnovRatio(test_ratios.aux_hvel[0]);
+	// Auxiliary velocity
+	test_ratios.aux_hvel[0] = _ekf.aid_src_aux_vel().test_ratio[0];
+	test_ratios.aux_hvel[1] = _ekf.aid_src_aux_vel().test_ratio[1];
 #endif // CONFIG_EKF2_AUXVEL
+
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	_ekf.getFlowInnovRatio(test_ratios.flow[0]);
+	// Optical flow
+	test_ratios.flow[0] = _ekf.aid_src_optical_flow().test_ratio[0];
+	test_ratios.flow[1] = _ekf.aid_src_optical_flow().test_ratio[1];
+# if defined(CONFIG_EKF2_TERRAIN)
+	test_ratios.terr_flow[0] = _ekf.aid_src_terrain_optical_flow().test_ratio[0];
+	test_ratios.terr_flow[1] = _ekf.aid_src_terrain_optical_flow().test_ratio[1];
+# endif // CONFIG_EKF2_TERRAIN
 #endif // CONFIG_EKF2_OPTICAL_FLOW
-	_ekf.getHeadingInnovRatio(test_ratios.heading);
+
+	// heading
+	test_ratios.heading = _ekf.getHeadingInnovRatio();
+
 #if defined(CONFIG_EKF2_MAGNETOMETER)
-	_ekf.getMagInnovRatio(test_ratios.mag_field[0]);
+	// mag_field
+	test_ratios.mag_field[0] = _ekf.aid_src_mag().test_ratio[0];
+	test_ratios.mag_field[1] = _ekf.aid_src_mag().test_ratio[1];
+	test_ratios.mag_field[2] = _ekf.aid_src_mag().test_ratio[2];
 #endif // CONFIG_EKF2_MAGNETOMETER
-#if defined(CONFIG_EKF2_DRAG_FUSION)
-	_ekf.getDragInnovRatio(&test_ratios.drag[0]);
-#endif // CONFIG_EKF2_DRAG_FUSION
-#if defined(CONFIG_EKF2_AIRSPEED)
-	_ekf.getAirspeedInnovRatio(test_ratios.airspeed);
-#endif // CONFIG_EKF2_AIRSPEED
-#if defined(CONFIG_EKF2_SIDESLIP)
-	_ekf.getBetaInnovRatio(test_ratios.beta);
-#endif // CONFIG_EKF2_SIDESLIP
 
 #if defined(CONFIG_EKF2_GRAVITY_FUSION)
-	_ekf.getGravityInnovRatio(test_ratios.gravity[0]);
+	// gravity
+	test_ratios.gravity[0] = _ekf.aid_src_gravity().test_ratio[0];
+	test_ratios.gravity[1] = _ekf.aid_src_gravity().test_ratio[1];
+	test_ratios.gravity[2] = _ekf.aid_src_gravity().test_ratio[2];
 #endif // CONFIG_EKF2_GRAVITY_FUSION
 
-#if defined(CONFIG_EKF2_TERRAIN)
+#if defined(CONFIG_EKF2_DRAG_FUSION)
-# if defined(CONFIG_EKF2_RANGE_FINDER)
+	// drag
-	_ekf.getHaglInnovRatio(test_ratios.hagl);
+	test_ratios.drag[0] = _ekf.aid_src_drag().test_ratio[0];
-# endif
+	test_ratios.drag[1] = _ekf.aid_src_drag().test_ratio[1];
-# if defined(CONFIG_EKF2_OPTICAL_FLOW)
+#endif // CONFIG_EKF2_DRAG_FUSION
-	_ekf.getTerrainFlowInnovRatio(test_ratios.terr_flow[0]);
-# endif // CONFIG_EKF2_OPTICAL_FLOW
-#endif // CONFIG_EKF2_TERRAIN
 
-	// Not yet supported
+#if defined(CONFIG_EKF2_AIRSPEED)
-	test_ratios.aux_vvel = NAN;
+	// airspeed
+	test_ratios.airspeed = _ekf.aid_src_airspeed().test_ratio;
+#endif // CONFIG_EKF2_AIRSPEED
+
+#if defined(CONFIG_EKF2_SIDESLIP)
+	// beta
+	test_ratios.beta = _ekf.aid_src_sideslip().test_ratio;
+#endif // CONFIG_EKF2_SIDESLIP
+
+#if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_RANGE_FINDER)
+	// hagl
+	test_ratios.hagl = _ekf.aid_src_terrain_range_finder().test_ratio;
+#endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER
+
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+	// hagl_rate
+	test_ratios.hagl_rate = _ekf.getHaglRateInnovRatio();
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 	test_ratios.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 	_estimator_innovation_test_ratios_pub.publish(test_ratios);
@@ -1559,54 +1637,93 @@ void EKF2::PublishInnovationVariances(const hrt_abstime &timestamp)
 	// publish estimator innovation variance data
 	estimator_innovations_s variances{};
 	variances.timestamp_sample = _ekf.time_delayed_us();
-#if defined(CONFIG_EKF2_GNSS)
-	_ekf.getGpsVelPosInnovVar(variances.gps_hvel, variances.gps_vvel, variances.gps_hpos, variances.gps_vpos);
-#endif // CONFIG_EKF2_GNSS
-#if defined(CONFIG_EKF2_EXTERNAL_VISION)
-	_ekf.getEvVelPosInnovVar(variances.ev_hvel, variances.ev_vvel, variances.ev_hpos, variances.ev_vpos);
-#endif // CONFIG_EKF2_EXTERNAL_VISION
-#if defined(CONFIG_EKF2_BAROMETER)
-	_ekf.getBaroHgtInnovVar(variances.baro_vpos);
-#endif // CONFIG_EKF2_BAROMETER
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-	_ekf.getRngHgtInnovVar(variances.rng_vpos);
-	_ekf.getHaglRateInnovVar(variances.hagl_rate);
-#endif // CONFIG_EKF2_RANGE_FINDER
-#if defined(CONFIG_EKF2_AUXVEL)
-	_ekf.getAuxVelInnovVar(variances.aux_hvel);
-#endif // CONFIG_EKF2_AUXVEL
-#if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	_ekf.getFlowInnovVar(variances.flow);
-#endif // CONFIG_EKF2_OPTICAL_FLOW
-	_ekf.getHeadingInnovVar(variances.heading);
-#if defined(CONFIG_EKF2_MAGNETOMETER)
-	_ekf.getMagInnovVar(variances.mag_field);
-#endif // CONFIG_EKF2_MAGNETOMETER
-#if defined(CONFIG_EKF2_DRAG_FUSION)
-	_ekf.getDragInnovVar(variances.drag);
-#endif // CONFIG_EKF2_DRAG_FUSION
-#if defined(CONFIG_EKF2_AIRSPEED)
-	_ekf.getAirspeedInnovVar(variances.airspeed);
-#endif // CONFIG_EKF2_AIRSPEED
-#if defined(CONFIG_EKF2_SIDESLIP)
-	_ekf.getBetaInnovVar(variances.beta);
-#endif // CONFIG_EKF2_SIDESLIP
 
-#if defined(CONFIG_EKF2_TERRAIN)
+#if defined(CONFIG_EKF2_GNSS)
-# if defined(CONFIG_EKF2_RANGE_FINDER)
+	// GPS
-	_ekf.getHaglInnovVar(variances.hagl);
+	variances.gps_hvel[0] = _ekf.aid_src_gnss_vel().innovation_variance[0];
-# endif // CONFIG_EKF2_RANGE_FINDER
+	variances.gps_hvel[1] = _ekf.aid_src_gnss_vel().innovation_variance[1];
-# if defined(CONFIG_EKF2_OPTICAL_FLOW)
+	variances.gps_vvel    = _ekf.aid_src_gnss_vel().innovation_variance[2];
-	_ekf.getTerrainFlowInnovVar(variances.terr_flow);
+	variances.gps_hpos[0] = _ekf.aid_src_gnss_pos().innovation_variance[0];
-# endif // CONFIG_EKF2_OPTICAL_FLOW
+	variances.gps_hpos[1] = _ekf.aid_src_gnss_pos().innovation_variance[1];
-#endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER
+	variances.gps_vpos    = _ekf.aid_src_gnss_hgt().innovation_variance;
+#endif // CONFIG_EKF2_GNSS
+
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
+	// External Vision
+	variances.ev_hvel[0] = _ekf.aid_src_ev_vel().innovation_variance[0];
+	variances.ev_hvel[1] = _ekf.aid_src_ev_vel().innovation_variance[1];
+	variances.ev_vvel    = _ekf.aid_src_ev_vel().innovation_variance[2];
+	variances.ev_hpos[0] = _ekf.aid_src_ev_pos().innovation_variance[0];
+	variances.ev_hpos[1] = _ekf.aid_src_ev_pos().innovation_variance[1];
+	variances.ev_vpos    = _ekf.aid_src_ev_hgt().innovation_variance;
+#endif // CONFIG_EKF2_EXTERNAL_VISION
+
+	// Height sensors
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+	variances.rng_vpos = _ekf.aid_src_rng_hgt().innovation_variance;
+#endif // CONFIG_EKF2_RANGE_FINDER
+#if defined(CONFIG_EKF2_BAROMETER)
+	variances.baro_vpos = _ekf.aid_src_baro_hgt().innovation_variance;
+#endif // CONFIG_EKF2_BAROMETER
+
+#if defined(CONFIG_EKF2_AUXVEL)
+	// Auxiliary velocity
+	variances.aux_hvel[0] = _ekf.aid_src_aux_vel().innovation_variance[0];
+	variances.aux_hvel[1] = _ekf.aid_src_aux_vel().innovation_variance[1];
+#endif // CONFIG_EKF2_AUXVEL
+
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
+	// Optical flow
+	variances.flow[0] = _ekf.aid_src_optical_flow().innovation_variance[0];
+	variances.flow[1] = _ekf.aid_src_optical_flow().innovation_variance[1];
+# if defined(CONFIG_EKF2_TERRAIN)
+	variances.terr_flow[0] = _ekf.aid_src_terrain_optical_flow().innovation_variance[0];
+	variances.terr_flow[1] = _ekf.aid_src_terrain_optical_flow().innovation_variance[1];
+# endif // CONFIG_EKF2_TERRAIN
+#endif // CONFIG_EKF2_OPTICAL_FLOW
+
+	// heading
+	variances.heading = _ekf.getHeadingInnovVar();
+
+#if defined(CONFIG_EKF2_MAGNETOMETER)
+	// mag_field
+	variances.mag_field[0] = _ekf.aid_src_mag().innovation_variance[0];
+	variances.mag_field[1] = _ekf.aid_src_mag().innovation_variance[1];
+	variances.mag_field[2] = _ekf.aid_src_mag().innovation_variance[2];
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_GRAVITY_FUSION)
-	_ekf.getGravityInnovVar(variances.gravity);
+	// gravity
+	variances.gravity[0] = _ekf.aid_src_gravity().innovation_variance[0];
+	variances.gravity[1] = _ekf.aid_src_gravity().innovation_variance[1];
+	variances.gravity[2] = _ekf.aid_src_gravity().innovation_variance[2];
 #endif // CONFIG_EKF2_GRAVITY_FUSION
 
-	// Not yet supported
+#if defined(CONFIG_EKF2_DRAG_FUSION)
-	variances.aux_vvel = NAN;
+	// drag
+	variances.drag[0] = _ekf.aid_src_drag().innovation_variance[0];
+	variances.drag[1] = _ekf.aid_src_drag().innovation_variance[1];
+#endif // CONFIG_EKF2_DRAG_FUSION
+
+#if defined(CONFIG_EKF2_AIRSPEED)
+	// airspeed
+	variances.airspeed = _ekf.aid_src_airspeed().innovation_variance;
+#endif // CONFIG_EKF2_AIRSPEED
+
+#if defined(CONFIG_EKF2_SIDESLIP)
+	// beta
+	variances.beta = _ekf.aid_src_sideslip().innovation_variance;
+#endif // CONFIG_EKF2_SIDESLIP
+
+#if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_RANGE_FINDER)
+	// hagl
+	variances.hagl = _ekf.aid_src_terrain_range_finder().innovation_variance;
+#endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER
+
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+	// hagl_rate
+	variances.hagl_rate = _ekf.getHaglRateInnovVar();
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 	variances.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 	_estimator_innovation_variances_pub.publish(variances);
@@ -2043,12 +2160,12 @@ void EKF2::PublishWindEstimate(const hrt_abstime &timestamp)
 		const Vector2f wind_vel_var = _ekf.getWindVelocityVariance();
 
 #if defined(CONFIG_EKF2_AIRSPEED)
-		_ekf.getAirspeedInnov(wind.tas_innov);
+		wind.tas_innov = _ekf.aid_src_airspeed().innovation;
-		_ekf.getAirspeedInnovVar(wind.tas_innov_var);
+		wind.tas_innov_var = _ekf.aid_src_airspeed().innovation_variance;
 #endif // CONFIG_EKF2_AIRSPEED
 #if defined(CONFIG_EKF2_SIDESLIP)
-		_ekf.getBetaInnov(wind.beta_innov);
+		wind.beta_innov = _ekf.aid_src_sideslip().innovation;
-		_ekf.getBetaInnovVar(wind.beta_innov_var);
+		wind.beta_innov = _ekf.aid_src_sideslip().innovation_variance;
 #endif // CONFIG_EKF2_SIDESLIP
 
 		wind.windspeed_north = wind_vel(0);

src/modules/ekf2/test/test_EKF_basics.cpp

@@ -245,22 +245,16 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_initialized)
 
 	_sensor_simulator.runSeconds(1);
 
-	float hpos = 0.f;
-	float vpos = 0.f;
-	float hvel = 0.f;
-	float vvel = 0.f;
-	float baro_vpos = 0.f;
-
 	// After the change of origin, the pos and vel innovations should stay small
-	_ekf->getGpsVelPosInnovRatio(hvel, vvel, hpos, vpos);
+	EXPECT_NEAR(_ekf->aid_src_gnss_pos().test_ratio[0], 0.f, 0.05f);
-	_ekf->getBaroHgtInnovRatio(baro_vpos);
+	EXPECT_NEAR(_ekf->aid_src_gnss_pos().test_ratio[1], 0.f, 0.05f);
+	EXPECT_NEAR(_ekf->aid_src_gnss_hgt().test_ratio, 0.f, 0.05f);
 
-	EXPECT_NEAR(hpos, 0.f, 0.05f);
+	EXPECT_NEAR(_ekf->aid_src_baro_hgt().test_ratio, 0.f, 0.05f);
-	EXPECT_NEAR(vpos, 0.f, 0.05f);
-	EXPECT_NEAR(baro_vpos, 0.f, 0.05f);
 
-	EXPECT_NEAR(hvel, 0.f, 0.02f);
+	EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[0], 0.f, 0.02f);
-	EXPECT_NEAR(vvel, 0.f, 0.02f);
+	EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[1], 0.f, 0.02f);
+	EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[2], 0.f, 0.02f);
 }
 
 TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized)
@@ -291,19 +285,14 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized)
 
 	_sensor_simulator.runSeconds(1);
 
-	float hpos = 0.f;
-	float vpos = 0.f;
-	float hvel = 0.f;
-	float vvel = 0.f;
-
 	// After the change of origin, the pos and vel innovations should stay small
-	_ekf->getGpsVelPosInnovRatio(hvel, vvel, hpos, vpos);
+	EXPECT_NEAR(_ekf->aid_src_gnss_pos().test_ratio[0], 0.f, 0.05f);
+	EXPECT_NEAR(_ekf->aid_src_gnss_pos().test_ratio[1], 0.f, 0.05f);
+	EXPECT_NEAR(_ekf->aid_src_gnss_hgt().test_ratio, 0.f, 0.05f);
 
-	EXPECT_NEAR(hpos, 0.f, 0.05f);
+	EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[0], 0.f, 0.02f);
-	EXPECT_NEAR(vpos, 0.f, 0.05f);
+	EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[1], 0.f, 0.02f);
-
+	EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[2], 0.f, 0.02f);
-	EXPECT_NEAR(hvel, 0.f, 0.02f);
-	EXPECT_NEAR(vvel, 0.f, 0.02f);
 }
 
 TEST_F(EkfBasicsTest, global_position_from_local_ev)

src/modules/ekf2/test/test_EKF_gps.cpp

@@ -222,8 +222,7 @@ TEST_F(EkfGpsTest, altitudeDrift)
 		_sensor_simulator.runSeconds(dt);
 	}
 
-	float baro_innov;
+	float baro_innov = _ekf->aid_src_baro_hgt().innovation;
-	_ekf->getBaroHgtInnov(baro_innov);
 	BiasEstimator::status status = _ekf->getBaroBiasEstimatorStatus();
 
 	printf("baro innov = %f\n", (double)baro_innov);

src/modules/ekf2/test/test_EKF_height_fusion.cpp

@@ -198,13 +198,8 @@ TEST_F(EkfHeightFusionTest, gpsRef)
 	EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, baro_initial + baro_increment - gps_step, 0.2f);
 
 	// and the innovations are close to zero
-	float baro_innov = NAN;
+	EXPECT_NEAR(_ekf->aid_src_baro_hgt().innovation, 0.f, 0.2f);
-	_ekf->getBaroHgtInnov(baro_innov);
+	EXPECT_NEAR(_ekf->aid_src_rng_hgt().innovation, 0.f, 0.2f);
-	EXPECT_NEAR(baro_innov, 0.f, 0.2f);
-
-	float rng_innov = NAN;
-	_ekf->getRngHgtInnov(rng_innov);
-	EXPECT_NEAR(rng_innov, 0.f, 0.2f);
 }
 
 TEST_F(EkfHeightFusionTest, baroRefFailOver)