WIP: ekf2: fake position fusion include height if no hgt mode enabled

src/modules/ekf2/EKF/control.cpp

@@ -507,7 +507,7 @@ void Ekf::controlOpticalFlowFusion()
 				// but use a relaxed time criteria to enable it to coast through bad range finder data
 				if (isRecent(_time_last_hagl_fuse, (uint64_t)10e6)) {
 					fuseOptFlow();
-					_last_known_posNE = _state.pos.xy();
+					_last_known_posNED = _state.pos;
 				}
 
 				_flow_data_ready = false;

src/modules/ekf2/EKF/ekf.h

@@ -401,7 +401,7 @@ private:
 	uint64_t _time_last_healthy_rng_data{0};
 	uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
 
-	Vector2f _last_known_posNE{};		///< last known local NE position vector (m)
+	Vector3f _last_known_posNED{};		///< last known local NED position vector (m)
 
 	uint64_t _time_acc_bias_check{0};	///< last time the  accel bias check passed (uSec)
 	uint64_t _delta_time_baro_us{0};	///< delta time between two consecutive delayed baro samples from the buffer (uSec)

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -179,7 +179,7 @@ void Ekf::resetHorizontalPositionToOpticalFlow()
 		resetHorizontalPositionTo(Vector2f(0.f, 0.f));
 
 	} else {
-		resetHorizontalPositionTo(_last_known_posNE);
+		resetHorizontalPositionTo(_last_known_posNED.xy());
 	}
 
 	// estimate is relative to initial position in this mode, so we start with zero error.
@@ -191,7 +191,7 @@ void Ekf::resetHorizontalPositionToLastKnown()
 	_information_events.flags.reset_pos_to_last_known = true;
 	ECL_INFO("reset position to last known position");
 	// Used when falling back to non-aiding mode of operation
-	resetHorizontalPositionTo(_last_known_posNE);
+	resetHorizontalPositionTo(_last_known_posNED.xy());
 	P.uncorrelateCovarianceSetVariance<2>(7, sq(_params.pos_noaid_noise));
 }
 

src/modules/ekf2/EKF/fake_pos_control.cpp

@@ -42,7 +42,8 @@ void Ekf::controlFakePosFusion()
 {
 	// If we aren't doing any aiding, fake position measurements at the last known position to constrain drift
 	// During intial tilt aligment, fake position is used to perform a "quasi-stationary" leveling of the EKF
-	const bool fake_pos_data_ready = isTimedOut(_time_last_fake_pos_fuse, (uint64_t)2e5); // Fuse fake position at a limited rate
+	const bool fake_pos_data_ready = isTimedOut(_time_last_fake_pos_fuse,
+					 (uint64_t)2e5); // Fuse fake position at a limited rate
 
 	if (fake_pos_data_ready) {
 		const bool continuing_conditions_passing = !isHorizontalAidingActive();
@@ -87,7 +88,7 @@ void Ekf::startFakePosFusion()
 
 void Ekf::resetFakePosFusion()
 {
-	_last_known_posNE = _state.pos.xy();
+	_last_known_posNED = _state.pos;
 	resetHorizontalPositionToLastKnown();
 	resetHorizontalVelocityToZero();
 	_time_last_fake_pos_fuse = _time_last_imu;
@@ -100,26 +101,40 @@ void Ekf::stopFakePosFusion()
 
 void Ekf::fuseFakePosition()
 {
-	Vector3f fake_pos_obs_var;
+	float obs_var = sq(0.5f);
+	Vector3f obs_var_3d;
 
 	if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
-		fake_pos_obs_var(0) = fake_pos_obs_var(1) = sq(fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise));
+		obs_var = sq(fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise));
 
 	} else if (!_control_status.flags.in_air && _control_status.flags.vehicle_at_rest) {
 		// Accelerate tilt fine alignment by fusing more
 		// aggressively when the vehicle is at rest
-		fake_pos_obs_var(0) = fake_pos_obs_var(1) = sq(0.01f);
-
-	} else {
-		fake_pos_obs_var(0) = fake_pos_obs_var(1) = sq(0.5f);
+		obs_var = sq(0.01f);
 	}
 
-	_gps_pos_innov.xy() = Vector2f(_state.pos) - _last_known_posNE;
+	obs_var_3d.setAll(obs_var);
 
-	const float fake_pos_innov_gate = 3.f;
+	_gps_pos_innov.xy() = Vector2f(_state.pos.xy()) - _last_known_posNED.xy();
 
-	if (fuseHorizontalPosition(_gps_pos_innov, fake_pos_innov_gate, fake_pos_obs_var,
-	                           _gps_pos_innov_var, _gps_pos_test_ratio, true)) {
+	const float innov_gate = 3.f;
+
+	if (fuseHorizontalPosition(_gps_pos_innov, innov_gate, obs_var_3d,
+				   _gps_pos_innov_var, _gps_pos_test_ratio, true)) {
 		_time_last_fake_pos_fuse = _time_last_imu;
 	}
+
+
+	if (!_control_status.flags.baro_hgt
+	    && !_control_status.flags.gps_hgt
+	    && !_control_status.flags.rng_hgt
+	    && !_control_status.flags.ev_hgt) {
+
+		if (isTimedOut(_time_last_hgt_fuse, (uint64_t)200e3)) {
+			fuseVerticalPosition(_gps_pos_innov(2), innov_gate, obs_var,
+					     _gps_pos_innov_var(2), _gps_pos_test_ratio(1));
+
+		}
+	}
+
 }