ekf2: estimator aid source status (GPS pos, GPS vel)

src/modules/ekf2/EKF/common.h

@@ -550,6 +550,10 @@ union information_event_status_u {
 		bool starting_vision_vel_fusion : 1; ///< 10 - true when the filter starts using vision system velocity measurements to correct the state estimates
 		bool starting_vision_yaw_fusion : 1; ///< 11 - true when the filter starts using vision system yaw  measurements to correct the state estimates
 		bool yaw_aligned_to_imu_gps     : 1; ///< 12 - true when the filter resets the yaw to an estimate derived from IMU and GPS data
+		bool reset_hgt_to_baro          : 1; ///< 13 - true when the vertical position state is reset to the baro measurement
+		bool reset_hgt_to_gps           : 1; ///< 14 - true when the vertical position state is reset to the gps measurement
+		bool reset_hgt_to_rng           : 1; ///< 15 - true when the vertical position state is reset to the rng measurement
+		bool reset_hgt_to_ev            : 1; ///< 16 - true when the vertical position state is reset to the ev measurement
 	} flags;
 	uint32_t value;
 };

src/modules/ekf2/EKF/control.cpp

@@ -778,6 +778,24 @@ void Ekf::checkVerticalAccelerationHealth()
 	bool is_inertial_nav_falling = false;
 	bool are_vertical_pos_and_vel_independant = false;
 
+	if (_control_status.flags.gps) {
+		// velocity
+		auto &gps_vel = _aid_src_gnss_vel;
+
+		if (gps_vel.time_last_fuse[2] > _vert_vel_fuse_time_us) {
+			_vert_vel_fuse_time_us = gps_vel.time_last_fuse[2];
+			_vert_vel_innov_ratio = gps_vel.innovation[2] / sqrtf(gps_vel.innovation_variance[2]);
+		}
+
+		// position
+		auto &gps_pos = _aid_src_gnss_pos;
+
+		if (gps_pos.time_last_fuse[2] > _vert_pos_fuse_attempt_time_us) {
+			_vert_pos_fuse_attempt_time_us = gps_pos.time_last_fuse[2];
+			_vert_pos_innov_ratio = gps_pos.innovation[2] / sqrtf(gps_pos.innovation_variance[2]);
+		}
+	}
+
 	if (isRecent(_vert_pos_fuse_attempt_time_us, 1000000)) {
 		if (isRecent(_vert_vel_fuse_time_us, 1000000)) {
 			// If vertical position and velocity come from independent sensors then we can
@@ -930,11 +948,7 @@ void Ekf::controlHeightFusion()
 		}
 
 	} else if (_control_status.flags.gps_hgt) {
-
-		if (_gps_data_ready) {
-			fuseGpsHgt();
-		}
-
+		// handled by gps fusion
 	} else if (_control_status.flags.rng_hgt) {
 
 		if (_range_sensor.isDataHealthy()) {

src/modules/ekf2/EKF/covariance.cpp

@@ -984,13 +984,24 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 		const float down_dvel_bias = _state.delta_vel_bias.dot(Vector3f(_R_to_earth.row(2)));
 
 		// check that the vertical component of accel bias is consistent with both the vertical position and velocity innovation
-		bool bad_acc_bias = (fabsf(down_dvel_bias) > dVel_bias_lim
-				     && ((down_dvel_bias * _gps_vel_innov(2) < 0.0f && _control_status.flags.gps)
-					 || (down_dvel_bias * _ev_vel_innov(2) < 0.0f && _control_status.flags.ev_vel))
-				     && ((down_dvel_bias * _gps_pos_innov(2) < 0.0f && _control_status.flags.gps_hgt)
-					 || (down_dvel_bias * _baro_hgt_innov < 0.0f && _control_status.flags.baro_hgt)
-					 || (down_dvel_bias * _rng_hgt_innov < 0.0f && _control_status.flags.rng_hgt)
-					 || (down_dvel_bias * _ev_pos_innov(2) < 0.0f && _control_status.flags.ev_hgt)));
+		bool bad_acc_bias = false;
+		if (fabsf(down_dvel_bias) > dVel_bias_lim) {
+
+			bool bad_vz_gps = _control_status.flags.gps    && (down_dvel_bias * _aid_src_gnss_vel.innovation[2] < 0.0f);
+			bool bad_vz_ev  = _control_status.flags.ev_vel && (down_dvel_bias * _ev_vel_innov(2) < 0.0f);
+
+			if (bad_vz_gps || bad_vz_ev) {
+				bool bad_z_baro = _control_status.flags.baro_hgt && (down_dvel_bias * _baro_hgt_innov < 0.0f);
+				bool bad_z_gps  = _control_status.flags.gps_hgt  && (down_dvel_bias * _aid_src_gnss_pos.innovation[2] < 0.0f);
+				bool bad_z_rng  = _control_status.flags.rng_hgt  && (down_dvel_bias * _rng_hgt_innov < 0.0f);
+				bool bad_z_ev   = _control_status.flags.ev_hgt   && (down_dvel_bias * _ev_pos_innov(2) < 0.0f);
+
+
+				if (bad_z_baro || bad_z_gps || bad_z_rng || bad_z_ev) {
+					bad_acc_bias = true;
+				}
+			}
+		}
 
 		// record the pass/fail
 		if (!bad_acc_bias) {

src/modules/ekf2/EKF/ekf.h

@@ -340,6 +340,9 @@ public:
 
 	const BaroBiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); }
 
+	const auto &aid_src_gnss_vel() const { return _aid_src_gnss_vel; }
+	const auto &aid_src_gnss_pos() const { return _aid_src_gnss_pos; }
+
 	const auto &aid_src_fake_pos() const { return _aid_src_fake_pos; }
 
 private:
@@ -443,12 +446,6 @@ private:
 	float _vert_vel_innov_ratio{0.f};		///< standard deviation of vertical velocity innovation
 	uint64_t _vert_vel_fuse_time_us{0};	///< last system time in usec time vertical velocity measurement fuson was attempted
 
-	Vector3f _gps_vel_innov{};	///< GPS velocity innovations (m/sec)
-	Vector3f _gps_vel_innov_var{};	///< GPS velocity innovation variances ((m/sec)**2)
-
-	Vector3f _gps_pos_innov{};	///< GPS position innovations (m)
-	Vector3f _gps_pos_innov_var{};	///< GPS position innovation variances (m**2)
-
 	Vector3f _ev_vel_innov{};	///< external vision velocity innovations (m/sec)
 	Vector3f _ev_vel_innov_var{};	///< external vision velocity innovation variances ((m/sec)**2)
 
@@ -496,6 +493,9 @@ private:
 	bool _inhibit_flow_use{false};	///< true when use of optical flow and range finder is being inhibited
 	Vector2f _flow_compensated_XY_rad{};	///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive
 
+	estimator_aid_source_3d_s _aid_src_gnss_vel{};
+	estimator_aid_source_3d_s _aid_src_gnss_pos{};
+
 	estimator_aid_source_2d_s _aid_src_fake_pos{};
 
 	// output predictor states
@@ -528,8 +528,12 @@ private:
 
 	// Variables used to perform in flight resets and switch between height sources
 	AlphaFilter<Vector3f> _mag_lpf{0.1f};	///< filtered magnetometer measurement for instant reset (Gauss)
-	float _hgt_sensor_offset{0.0f};		///< set as necessary if desired to maintain the same height after a height reset (m)
+
 	float _baro_hgt_offset{0.0f};		///< baro height reading at the local NED origin (m)
+	float _gps_hgt_offset{0.0f};		///< GPS height reading at the local NED origin (m)
+	float _rng_hgt_offset{0.0f};		///< Range height reading at the local NED origin (m)
+	float _ev_hgt_offset{0.0f};		///< EV height reading at the local NED origin (m)
+
 	float _baro_hgt_bias{0.0f};
 	float _baro_hgt_bias_var{1.f};
 
@@ -641,7 +645,6 @@ private:
 	void fuseDrag(const dragSample &drag_sample);
 
 	void fuseBaroHgt();
-	void fuseGpsHgt();
 	void fuseRngHgt();
 	void fuseEvHgt();
 
@@ -688,7 +691,11 @@ private:
 	bool fuseVerticalPosition(float innov, float innov_gate, float obs_var,
 				  float &innov_var, float &test_ratio);
 
-	void fuseGpsVelPos();
+	void updateGpsVel(const gpsSample &gps_sample);
+	void fuseGpsVel();
+
+	void updateGpsPos(const gpsSample &gps_sample);
+	void fuseGpsPos();
 
 	// calculate optical flow body angular rate compensation
 	// returns false if bias corrected body rate data is unavailable

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -241,7 +241,10 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos)
 
 void Ekf::resetHeightToBaro()
 {
-	resetVerticalPositionTo(-_baro_sample_delayed.hgt + _baro_hgt_offset);
+	ECL_INFO("reset height to baro");
+	_information_events.flags.reset_hgt_to_baro = true;
+
+	resetVerticalPositionTo(-(_baro_sample_delayed.hgt - _baro_hgt_offset));
 
 	// the state variance is the same as the observation
 	P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.baro_noise));
@@ -249,6 +252,9 @@ void Ekf::resetHeightToBaro()
 
 void Ekf::resetHeightToGps()
 {
+	ECL_INFO("reset height to GPS");
+	_information_events.flags.reset_hgt_to_gps = true;
+
 	const float z_pos_before_reset = _state.pos(2);
 	resetVerticalPositionTo(-_gps_sample_delayed.hgt + _gps_alt_ref);
 
@@ -261,6 +267,9 @@ void Ekf::resetHeightToGps()
 
 void Ekf::resetHeightToRng()
 {
+	ECL_INFO("reset height to RNG");
+	_information_events.flags.reset_hgt_to_rng = true;
+
 	float dist_bottom;
 
 	if (_control_status.flags.in_air) {
@@ -273,7 +282,7 @@ void Ekf::resetHeightToRng()
 
 	// update the state and associated variance
 	const float z_pos_before_reset = _state.pos(2);
-	resetVerticalPositionTo(-dist_bottom + _hgt_sensor_offset);
+	resetVerticalPositionTo(-dist_bottom + _rng_hgt_offset);
 
 	// the state variance is the same as the observation
 	P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.range_noise));
@@ -284,6 +293,9 @@ void Ekf::resetHeightToRng()
 
 void Ekf::resetHeightToEv()
 {
+	ECL_INFO("reset height to EV");
+	_information_events.flags.reset_hgt_to_ev = true;
+
 	const float z_pos_before_reset = _state.pos(2);
 	resetVerticalPositionTo(_ev_sample_delayed.pos(2));
 
@@ -352,7 +364,8 @@ bool Ekf::realignYawGPS(const Vector3f &mag)
 	}
 
 	// check for excessive horizontal GPS velocity innovations
-	const bool badVelInnov = (_gps_vel_test_ratio(0) > 1.0f) && _control_status.flags.gps;
+	const float gps_vel_test_ratio = fmaxf(_aid_src_gnss_vel.test_ratio[0], _aid_src_gnss_vel.test_ratio[1]);
+	const bool badVelInnov = (gps_vel_test_ratio > 1.0f) && _control_status.flags.gps;
 
 	// calculate GPS course over ground angle
 	const float gpsCOG = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0));
@@ -593,30 +606,33 @@ Vector3f Ekf::calcEarthRateNED(float lat_rad) const
 
 void Ekf::getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2],  float &vpos) const
 {
-	hvel[0] = _gps_vel_innov(0);
-	hvel[1] = _gps_vel_innov(1);
-	vvel    = _gps_vel_innov(2);
-	hpos[0] = _gps_pos_innov(0);
-	hpos[1] = _gps_pos_innov(1);
-	vpos    = _gps_pos_innov(2);
+	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_pos.innovation[2];
 }
 
 void Ekf::getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos)  const
 {
-	hvel[0] = _gps_vel_innov_var(0);
-	hvel[1] = _gps_vel_innov_var(1);
-	vvel    = _gps_vel_innov_var(2);
-	hpos[0] = _gps_pos_innov_var(0);
-	hpos[1] = _gps_pos_innov_var(1);
-	vpos    = _gps_pos_innov_var(2);
+	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_pos.innovation_variance[2];
 }
 
 void Ekf::getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const
 {
-	hvel = _gps_vel_test_ratio(0);
-	vvel = _gps_vel_test_ratio(1);
-	hpos = _gps_pos_test_ratio(0);
-	vpos = _gps_pos_test_ratio(1);
+	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_pos.test_ratio[2];
 }
 
 void Ekf::getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const
@@ -728,7 +744,7 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const
 	// and using state variances for accuracy reporting is overly optimistic in these situations
 	if (_control_status.flags.inertial_dead_reckoning) {
 		if (_control_status.flags.gps) {
-			hpos_err = math::max(hpos_err, sqrtf(sq(_gps_pos_innov(0)) + sq(_gps_pos_innov(1))));
+			hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm());
 		}
 
 		if (_control_status.flags.ev_pos) {
@@ -750,7 +766,7 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const
 	// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
 	// and using state variances for accuracy reporting is overly optimistic in these situations
 	if (_deadreckon_time_exceeded && _control_status.flags.gps) {
-		hpos_err = math::max(hpos_err, sqrtf(sq(_gps_pos_innov(0)) + sq(_gps_pos_innov(1))));
+		hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm());
 	}
 
 	*ekf_eph = hpos_err;
@@ -774,7 +790,7 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
 		}
 
 		if (_control_status.flags.gps) {
-			vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_gps_pos_innov(0)) + sq(_gps_pos_innov(1))));
+			vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_gnss_pos.innovation).norm());
 
 		} else if (_control_status.flags.ev_pos) {
 			vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_ev_pos_innov(0)) + sq(_ev_pos_innov(1))));
@@ -901,10 +917,10 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 	pos = NAN;
 
 	if (_control_status.flags.gps) {
-		float gps_vel = sqrtf(math::max(_gps_vel_test_ratio(0), _gps_vel_test_ratio(1)));
+		float gps_vel = sqrtf(Vector3f(_aid_src_gnss_vel.test_ratio).max());
 		vel = math::max(gps_vel, FLT_MIN);
 
-		float gps_pos = sqrtf(_gps_pos_test_ratio(0));
+		float gps_pos = sqrtf(Vector2f(_aid_src_gnss_pos.test_ratio).max());
 		pos = math::max(gps_pos, FLT_MIN);
 	}
 
@@ -928,7 +944,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 		hgt = math::max(sqrtf(_baro_hgt_test_ratio), FLT_MIN);
 
 	} else if (_control_status.flags.gps_hgt) {
-		hgt = math::max(sqrtf(_gps_pos_test_ratio(1)), FLT_MIN);
+		hgt = math::max(sqrtf(_aid_src_gnss_pos.test_ratio[2]), FLT_MIN);
 
 	} else if (_control_status.flags.rng_hgt) {
 		hgt = math::max(sqrtf(_rng_hgt_test_ratio), FLT_MIN);
@@ -965,8 +981,8 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
 	soln_status.flags.const_pos_mode = !soln_status.flags.velocity_horiz;
 	soln_status.flags.pred_pos_horiz_rel = soln_status.flags.pos_horiz_rel;
 	soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_horiz_abs;
-	const bool gps_vel_innov_bad = (_gps_vel_test_ratio(0) > 1.0f) || (_gps_vel_test_ratio(1) > 1.0f);
-	const bool gps_pos_innov_bad = (_gps_pos_test_ratio(0) > 1.0f);
+	const bool gps_vel_innov_bad = Vector3f(_aid_src_gnss_vel.test_ratio).max() > 1.f;
+	const bool gps_pos_innov_bad = Vector2f(_aid_src_gnss_pos.test_ratio).max() > 1.f;
 	const bool mag_innov_good = (_mag_test_ratio.max() < 1.0f) && (_yaw_test_ratio < 1.0f);
 	soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad) && mag_innov_good;
 	soln_status.flags.accel_error = _fault_status.flags.bad_acc_vertical;
@@ -1000,6 +1016,7 @@ void Ekf::update_deadreckoning_status()
 				      || isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max));
 
 	const bool optFlowAiding = _control_status.flags.opt_flow && isRecent(_time_last_of_fuse, _params.no_aid_timeout_max);
+
 	const bool airDataAiding = _control_status.flags.wind &&
 				   isRecent(_time_last_arsp_fuse, _params.no_aid_timeout_max) &&
 				   isRecent(_time_last_beta_fuse, _params.no_aid_timeout_max);
@@ -1254,7 +1271,8 @@ void Ekf::startBaroHgtFusion()
 
 		// We don't need to set a height sensor offset
 		// since we track a separate _baro_hgt_offset
-		_hgt_sensor_offset = 0.0f;
+
+		ECL_INFO("starting baro height fusion");
 	}
 }
 
@@ -1262,17 +1280,19 @@ void Ekf::startGpsHgtFusion()
 {
 	if (!_control_status.flags.gps_hgt) {
 		if (_control_status.flags.rng_hgt) {
-			// swith out of range aid
+			// switch out of range aid
 			// calculate height sensor offset such that current
 			// measurement matches our current height estimate
-			_hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2);
+			_gps_hgt_offset = (_gps_sample_delayed.hgt - _gps_alt_ref) + _state.pos(2);
 
 		} else {
-			_hgt_sensor_offset = 0.f;
+			_gps_hgt_offset = 0.f;
 			resetHeightToGps();
 		}
 
 		setControlGPSHeight();
+
+		ECL_INFO("starting GPS height fusion");
 	}
 }
 
@@ -1283,12 +1303,14 @@ void Ekf::startRngHgtFusion()
 
 		// Range finder is the primary height source, the ground is now the datum used
 		// to compute the local vertical position
-		_hgt_sensor_offset = 0.f;
+		_rng_hgt_offset = 0.f;
 
 		if (!_control_status_prev.flags.ev_hgt) {
 			// EV and range finders are using the same height datum
 			resetHeightToRng();
 		}
+
+		ECL_INFO("starting RNG height fusion");
 	}
 }
 
@@ -1299,7 +1321,9 @@ void Ekf::startRngAidHgtFusion()
 
 		// calculate height sensor offset such that current
 		// measurement matches our current height estimate
-		_hgt_sensor_offset = _terrain_vpos;
+		_rng_hgt_offset = _terrain_vpos;
+
+		ECL_INFO("starting RNG aid height fusion");
 	}
 }
 
@@ -1312,6 +1336,8 @@ void Ekf::startEvHgtFusion()
 			// EV and range finders are using the same height datum
 			resetHeightToEv();
 		}
+
+		ECL_INFO("starting EV height fusion");
 	}
 }
 
@@ -1557,6 +1583,8 @@ void Ekf::stopGpsFusion()
 	if (_control_status.flags.gps) {
 		stopGpsPosFusion();
 		stopGpsVelFusion();
+
+		_control_status.flags.gps = false;
 	}
 
 	if (_control_status.flags.gps_yaw) {
@@ -1570,27 +1598,27 @@ void Ekf::stopGpsFusion()
 
 void Ekf::stopGpsPosFusion()
 {
-	_control_status.flags.gps = false;
+	ECL_INFO("stopping GPS position fusion");
 
 	if (_control_status.flags.gps_hgt) {
+		ECL_INFO("stopping GPS height fusion");
 		startBaroHgtFusion();
 	}
 
-	_gps_pos_innov.setZero();
-	_gps_pos_innov_var.setZero();
-	_gps_pos_test_ratio.setZero();
+	resetEstimatorAidStatus(_aid_src_gnss_pos);
 }
 
 void Ekf::stopGpsVelFusion()
 {
-	_gps_vel_innov.setZero();
-	_gps_vel_innov_var.setZero();
-	_gps_vel_test_ratio.setZero();
+	ECL_INFO("stopping GPS velocity fusion");
+
+	resetEstimatorAidStatus(_aid_src_gnss_vel);
 }
 
 void Ekf::startGpsYawFusion()
 {
-	if (resetYawToGps()) {
+	if (!_control_status.flags.gps_yaw && resetYawToGps()) {
+		ECL_INFO("starting GPS yaw fusion");
 		_control_status.flags.yaw_align = true;
 		_control_status.flags.mag_dec = false;
 		stopEvYawFusion();
@@ -1598,12 +1626,14 @@ void Ekf::startGpsYawFusion()
 		stopMag3DFusion();
 		_control_status.flags.gps_yaw = true;
 	}
-
 }
 
 void Ekf::stopGpsYawFusion()
 {
-	_control_status.flags.gps_yaw = false;
+	if (_control_status.flags.gps_yaw) {
+		ECL_INFO("stopping GPS yaw fusion");
+		_control_status.flags.gps_yaw = false;
+	}
 }
 
 void Ekf::startEvPosFusion()

src/modules/ekf2/EKF/estimator_interface.h

@@ -329,10 +329,8 @@ protected:
 
 	// innovation consistency check monitoring ratios
 	float _yaw_test_ratio{};		// yaw innovation consistency check ratio
-	AlphaFilter<float>_yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state
+	AlphaFilter<float> _yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state
 	Vector3f _mag_test_ratio{};		// magnetometer XYZ innovation consistency check ratios
-	Vector2f _gps_vel_test_ratio{};		// GPS velocity innovation consistency check ratios
-	Vector2f _gps_pos_test_ratio{};		// GPS position innovation consistency check ratios
 	Vector2f _ev_vel_test_ratio{};		// EV velocity innovation consistency check ratios
 	Vector2f _ev_pos_test_ratio{};		// EV position innovation consistency check ratios
 	Vector2f _aux_vel_test_ratio{};		// Auxiliary horizontal velocity innovation consistency check ratio

src/modules/ekf2/EKF/gps_control.cpp

@@ -48,6 +48,14 @@ void Ekf::controlGpsFusion()
 
 	// Check for new GPS data that has fallen behind the fusion time horizon
 	if (_gps_data_ready) {
+
+		// reset flags
+		resetEstimatorAidStatusFlags(_aid_src_gnss_vel);
+		resetEstimatorAidStatusFlags(_aid_src_gnss_pos);
+
+		updateGpsVel(_gps_sample_delayed);
+		updateGpsPos(_gps_sample_delayed);
+
 		const bool gps_checks_passing = isTimedOut(_last_gps_fail_us, (uint64_t)5e6);
 		const bool gps_checks_failing = isTimedOut(_last_gps_pass_us, (uint64_t)5e6);
 
@@ -67,7 +75,8 @@ void Ekf::controlGpsFusion()
 				if (continuing_conditions_passing
 				    || !isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
 
-					fuseGpsVelPos();
+					fuseGpsVel();
+					fuseGpsPos();
 
 					if (shouldResetGpsFusion()) {
 						const bool was_gps_signal_lost = isTimedOut(_time_prev_gps_us, 1000000);

src/modules/ekf2/EKF/gps_fusion.cpp

@@ -39,39 +39,145 @@
 /* #include <mathlib/mathlib.h> */
 #include "ekf.h"
 
-void Ekf::fuseGpsVelPos()
+void Ekf::updateGpsVel(const gpsSample &gps_sample)
 {
-	Vector3f gps_pos_obs_var;
+	const float vel_var = sq(gps_sample.sacc);
+	const Vector3f obs_var{vel_var, vel_var, vel_var * sq(1.5f)};
+
+	// innovation gate size
+	const float innov_gate = fmaxf(_params.gps_vel_innov_gate, 1.f);
+
+	auto &gps_vel = _aid_src_gnss_vel;
+
+	for (int i = 0; i < 3; i++) {
+		gps_vel.observation[i] = gps_sample.vel(i);
+		gps_vel.observation_variance[i] = obs_var(i);
+
+		gps_vel.innovation[i] = _state.vel(i) - gps_sample.vel(i);
+		gps_vel.innovation_variance[i] = P(4 + i, 4 + i) + obs_var(i);
+		gps_vel.test_ratio[i] = sq(gps_vel.innovation[i]) / (sq(innov_gate) * gps_vel.innovation_variance[i]);
+
+		gps_vel.innovation_rejected[i] = (gps_vel.test_ratio[i] > 1.f);
+	}
+
+	// vz special case if there is bad vertical acceleration data, then don't reject measurement,
+	// but limit innovation to prevent spikes that could destabilise the filter
+	if (_fault_status.flags.bad_acc_vertical && gps_vel.innovation_rejected[2]) {
+		const float innov_limit = innov_gate * sqrtf(gps_vel.innovation_variance[2]);
+		gps_vel.innovation[2] = math::constrain(gps_vel.innovation[2], -innov_limit, innov_limit);
+		gps_vel.innovation_rejected[2] = false;
+	}
+
+	gps_vel.timestamp_sample = gps_sample.time_us;
+}
+
+void Ekf::updateGpsPos(const gpsSample &gps_sample)
+{
+	Vector3f position;
+	position(0) = gps_sample.pos(0);
+	position(1) = gps_sample.pos(1);
+
+	// vertical position - gps measurement has opposite sign to earth z axis
+	position(2) = -(gps_sample.hgt - _gps_alt_ref - _gps_hgt_offset);
 
 	const float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
 
+	Vector3f obs_var;
+
 	if (isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
 		// if we are using other sources of aiding, then relax the upper observation
 		// noise limit which prevents bad GPS perturbing the position estimate
-		gps_pos_obs_var(0) = gps_pos_obs_var(1) = sq(fmaxf(_gps_sample_delayed.hacc, lower_limit));
+		obs_var(0) = obs_var(1) = sq(fmaxf(gps_sample.hacc, lower_limit));
 
 	} else {
 		// if we are not using another source of aiding, then we are reliant on the GPS
 		// observations to constrain attitude errors and must limit the observation noise value.
 		float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
-		gps_pos_obs_var(0) = gps_pos_obs_var(1) = sq(math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit));
+		obs_var(0) = obs_var(1) = sq(math::constrain(gps_sample.hacc, lower_limit, upper_limit));
 	}
 
+	obs_var(2) = getGpsHeightVariance();
 
+	// innovation gate size
+	float innov_gate = fmaxf(_params.gps_pos_innov_gate, 1.f);
 
-	const float vel_var = sq(_gps_sample_delayed.sacc);
-	const Vector3f gps_vel_obs_var{vel_var, vel_var, vel_var * sq(1.5f)};
+	auto &gps_pos = _aid_src_gnss_pos;
 
-	// calculate innovations
-	_gps_vel_innov = _state.vel - _gps_sample_delayed.vel;
-	_gps_pos_innov.xy() = Vector2f(_state.pos) - _gps_sample_delayed.pos;
+	for (int i = 0; i < 3; i++) {
+		gps_pos.observation[i] = position(i);
+		gps_pos.observation_variance[i] = obs_var(i);
 
-	// set innovation gate size
-	const float pos_innov_gate = fmaxf(_params.gps_pos_innov_gate, 1.f);
-	const float vel_innov_gate = fmaxf(_params.gps_vel_innov_gate, 1.f);
+		gps_pos.innovation[i] = _state.pos(i) - position(i);
+		gps_pos.innovation_variance[i] = P(7 + i, 7 + i) + obs_var(i);
+		gps_pos.test_ratio[i] = sq(gps_pos.innovation[i]) / (sq(innov_gate) * gps_pos.innovation_variance[i]);
 
-	// fuse GPS measurement
-	fuseHorizontalVelocity(_gps_vel_innov, vel_innov_gate, gps_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio);
-	fuseVerticalVelocity(_gps_vel_innov, vel_innov_gate, gps_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio);
-	fuseHorizontalPosition(_gps_pos_innov, pos_innov_gate, gps_pos_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio);
+		gps_pos.innovation_rejected[i] = (gps_pos.test_ratio[i] > 1.f);
+	}
+
+	// z special case if there is bad vertical acceleration data, then don't reject measurement,
+	// but limit innovation to prevent spikes that could destabilise the filter
+	if (_fault_status.flags.bad_acc_vertical && gps_pos.innovation_rejected[2]) {
+		const float innov_limit = innov_gate * sqrtf(gps_pos.innovation_variance[2]);
+		gps_pos.innovation[2] = math::constrain(gps_pos.innovation[2], -innov_limit, innov_limit);
+		gps_pos.innovation_rejected[2] = false;
+	}
+
+	gps_pos.timestamp_sample = gps_sample.time_us;
+}
+
+void Ekf::fuseGpsVel()
+{
+	// velocity
+	auto &gps_vel = _aid_src_gnss_vel;
+
+	// vx & vy
+	gps_vel.fusion_enabled[0] = true;
+	gps_vel.fusion_enabled[1] = true;
+
+	if (!gps_vel.innovation_rejected[0] && !gps_vel.innovation_rejected[1]) {
+		for (int i = 0; i < 2; i++) {
+			if (fuseVelPosHeight(gps_vel.innovation[i], gps_vel.innovation_variance[i], i)) {
+				gps_vel.fused[i] = true;
+				gps_vel.time_last_fuse[i] = _time_last_imu;
+			}
+		}
+	}
+
+	// vz
+	gps_vel.fusion_enabled[2] = true;
+
+	if (gps_vel.fusion_enabled[2] && !gps_vel.innovation_rejected[2]) {
+		if (fuseVelPosHeight(gps_vel.innovation[2], gps_vel.innovation_variance[2], 2)) {
+			gps_vel.fused[2] = true;
+			gps_vel.time_last_fuse[2] = _time_last_imu;
+		}
+	}
+}
+
+void Ekf::fuseGpsPos()
+{
+	auto &gps_pos = _aid_src_gnss_pos;
+
+	// x & y
+	gps_pos.fusion_enabled[0] = true;
+	gps_pos.fusion_enabled[1] = true;
+
+	if (!gps_pos.innovation_rejected[0] && !gps_pos.innovation_rejected[1]) {
+		for (int i = 0; i < 2; i++) {
+			if (fuseVelPosHeight(gps_pos.innovation[i], gps_pos.innovation_variance[i], 3 + i)) {
+				gps_pos.fused[i] = true;
+				gps_pos.time_last_fuse[i] = _time_last_imu;
+			}
+		}
+	}
+
+	// z
+	gps_pos.fusion_enabled[2] = _control_status.flags.gps_hgt;
+
+	if (gps_pos.fusion_enabled[2] && !gps_pos.innovation_rejected[2]) {
+		if (fuseVelPosHeight(gps_pos.innovation[2], gps_pos.innovation_variance[2], 5)) {
+			gps_pos.fused[2] = true;
+			gps_pos.time_last_fuse[2] = _time_last_imu;
+		}
+	}
 }

src/modules/ekf2/EKF/height_fusion.cpp

@@ -72,26 +72,11 @@ void Ekf::fuseBaroHgt()
 			     _baro_hgt_innov_var, _baro_hgt_test_ratio);
 }
 
-void Ekf::fuseGpsHgt()
-{
-	// vertical position innovation - gps measurement has opposite sign to earth z axis
-	_gps_pos_innov(2) = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset;
-
-	// innovation gate size
-	float innov_gate = fmaxf(_params.baro_innov_gate, 1.f);
-
-	float obs_var = getGpsHeightVariance();
-
-	// _gps_pos_test_ratio(1) is the vertical test ratio
-	fuseVerticalPosition(_gps_pos_innov(2), innov_gate, obs_var,
-			     _gps_pos_innov_var(2), _gps_pos_test_ratio(1));
-}
-
 void Ekf::fuseRngHgt()
 {
 	// use range finder with tilt correction
 	_rng_hgt_innov = _state.pos(2) - (-math::max(_range_sensor.getDistBottom(),
-					  _params.rng_gnd_clearance)) - _hgt_sensor_offset;
+					  _params.rng_gnd_clearance)) - _rng_hgt_offset;
 
 	// innovation gate size
 	float innov_gate = fmaxf(_params.range_innov_gate, 1.f);