EKF2: reset position by fusion (#23279)

* reset position by fusion

* handle local_pos_valid for fixed wing in gnss denied

* [WIP] ekf2: setEkfGlobalOrigin respect current height reference and vertical position aiding

* global origin, also reset vertical pos without gps as ref

* fix wo gnss, that bitcraze ci passes

* revert some changes as requested

* remove duplicate reset messages

* undo unrelated whitespace changes, I'll fix it everywhere in a followup

* [SQUASH] ekf2: add vehicle_command_ack

* resetGlobalPosToExternalObservation consolidate logic

* remove gnss check from local_pos validation check

* reset when  0<accuracy<1, otherwise fuse

* replace gps param with flag

* ekf2: dead reckon time exceeded, respect ZUPT preflight if air data or optical flow expected

* subtract timeout from last inertial dead-reck, change fake pos conditions, save flash

---------

Co-authored-by: Daniel Agar <daniel@agar.ca>

ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu

@@ -7,6 +7,7 @@
 #
 # @maintainer
 # @board px4_fmu-v2 exclude
+# @board px4_fmu-v5x exclude
 # @board px4_fmu-v6x exclude
 #
 

src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp

@@ -42,13 +42,13 @@ ZeroVelocityUpdate::ZeroVelocityUpdate()
 
 void ZeroVelocityUpdate::reset()
 {
-	_time_last_zero_velocity_fuse = 0;
+	_time_last_fuse = 0;
 }
 
 bool ZeroVelocityUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
 {
 	// Fuse zero velocity at a limited rate (every 200 milliseconds)
-	const bool zero_velocity_update_data_ready = (_time_last_zero_velocity_fuse + 200'000 < imu_delayed.time_us);
+	const bool zero_velocity_update_data_ready = (_time_last_fuse + 200'000 < imu_delayed.time_us);
 
 	if (zero_velocity_update_data_ready) {
 		const bool continuing_conditions_passing = ekf.control_status_flags().vehicle_at_rest
@@ -69,7 +69,7 @@ bool ZeroVelocityUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delaye
 				ekf.fuseDirectStateMeasurement(innovation, innov_var(i), obs_var, State::vel.idx + i);
 			}
 
-			_time_last_zero_velocity_fuse = imu_delayed.time_us;
+			_time_last_fuse = imu_delayed.time_us;
 
 			return true;
 		}

src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.hpp

@@ -45,9 +45,11 @@ public:
 	void reset() override;
 	bool update(Ekf &ekf, const estimator::imuSample &imu_delayed) override;
 
+	const auto &time_last_fuse() const { return _time_last_fuse; }
+
 private:
 
-	uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec)
+	uint64_t _time_last_fuse{0}; ///< last time of zero velocity update (uSec)
 
 };
 

src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp

@@ -75,15 +75,13 @@ void Ekf::controlFakePosFusion()
 					 Vector2f(getStateVariance<State::pos>()) + obs_var, // innovation variance
 					 innov_gate);                                        // innovation gate
 
-		const bool continuing_conditions_passing = !isHorizontalAidingActive()
+		const bool enable_conditions_passing = !isHorizontalAidingActive()
 				&& ((getTiltVariance() > sq(math::radians(3.f))) || _control_status.flags.vehicle_at_rest)
-				&& (!(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector)) || _control_status.flags.vehicle_at_rest);
-
-		const bool starting_conditions_passing = continuing_conditions_passing
+				&& (!(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector)) || _control_status.flags.vehicle_at_rest)
 				&& _horizontal_deadreckon_time_exceeded;
 
 		if (_control_status.flags.fake_pos) {
-			if (continuing_conditions_passing) {
+			if (enable_conditions_passing) {
 
 				// always protect against extreme values that could result in a NaN
 				if ((aid_src.test_ratio[0] < sq(100.0f / innov_gate))
@@ -104,7 +102,7 @@ void Ekf::controlFakePosFusion()
 			}
 
 		} else {
-			if (starting_conditions_passing) {
+			if (enable_conditions_passing) {
 				ECL_INFO("start fake position fusion");
 				_control_status.flags.fake_pos = true;
 				resetFakePosFusion();

src/modules/ekf2/EKF/ekf.cpp

@@ -281,23 +281,66 @@ void Ekf::predictState(const imuSample &imu_delayed)
 	_height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf;
 }
 
-void Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation)
+bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation)
 {
 	if (!_pos_ref.isInitialized()) {
-		return;
+		ECL_WARN("unable to reset global position, position reference not initialized");
+		return false;
 	}
 
-	// apply a first order correction using velocity at the delated time horizon and the delta time
-	timestamp_observation = math::min(_time_latest_us, timestamp_observation);
-	const float dt = _time_delayed_us > timestamp_observation ? static_cast<float>(_time_delayed_us - timestamp_observation)
-			 * 1e-6f : -static_cast<float>(timestamp_observation - _time_delayed_us) * 1e-6f;
+	Vector2f pos_corrected = _pos_ref.project(lat_deg, lon_deg);
 
-	Vector2f pos_corrected = _pos_ref.project(lat_deg, lon_deg) + _state.vel.xy() * dt;
+	// apply a first order correction using velocity at the delayed time horizon and the delta time
+	if ((timestamp_observation > 0) && (isHorizontalAidingActive() || !_horizontal_deadreckon_time_exceeded)) {
 
-	resetHorizontalPositionTo(pos_corrected, sq(math::max(accuracy, 0.01f)));
+		timestamp_observation = math::min(_time_latest_us, timestamp_observation);
 
-	ECL_INFO("reset position to external observation");
-	_information_events.flags.reset_pos_to_ext_obs = true;
+		float diff_us = 0.f;
+
+		if (_time_delayed_us >= timestamp_observation) {
+			diff_us = static_cast<float>(_time_delayed_us - timestamp_observation);
+
+		} else {
+			diff_us = -static_cast<float>(timestamp_observation - _time_delayed_us);
+		}
+
+		const float dt_s = diff_us * 1e-6f;
+		pos_corrected += _state.vel.xy() * dt_s;
+	}
+
+	const float obs_var = math::max(accuracy, sq(0.01f));
+
+	const Vector2f innov = Vector2f(_state.pos.xy()) - pos_corrected;
+	const Vector2f innov_var = Vector2f(getStateVariance<State::vel>()) + obs_var;
+
+	const float sq_gate = sq(5.f); // magic hardcoded gate
+	const Vector2f test_ratio{sq(innov(0)) / (sq_gate * innov_var(0)),
+					sq(innov(1)) / (sq_gate * innov_var(1))};
+
+	const bool innov_rejected = (test_ratio.max() > 1.f);
+
+	if (!_control_status.flags.in_air || (accuracy > 0.f && accuracy < 1.f) || innov_rejected) {
+		// when on ground or accuracy chosen to be very low, we hard reset position
+		// this allows the user to still send hard resets at any time
+		ECL_INFO("reset position to external observation");
+		_information_events.flags.reset_pos_to_ext_obs = true;
+
+		resetHorizontalPositionTo(pos_corrected, obs_var);
+		_last_known_pos.xy() = _state.pos.xy();
+		return true;
+
+	} else {
+		if (fuseDirectStateMeasurement(innov(0), innov_var(0), obs_var, State::pos.idx + 0)
+		 && fuseDirectStateMeasurement(innov(1), innov_var(1), obs_var, State::pos.idx + 1)
+		) {
+			ECL_INFO("fused external observation as position measurement");
+			_time_last_hor_pos_fuse = _time_delayed_us;
+			_last_known_pos.xy() = _state.pos.xy();
+			return true;
+		}
+	}
+
+	return false;
 }
 
 void Ekf::updateParameters()

src/modules/ekf2/EKF/ekf.h

@@ -501,7 +501,7 @@ public:
 		return true;
 	}
 
-	void resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation);
+	bool resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation);
 
 	void updateParameters();
 

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -89,7 +89,7 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
 			current_pos_available = true;
 		}
 
-		const float gps_alt_ref_prev = getEkfGlobalOriginAltitude();
+		const float gps_alt_ref_prev = _gps_alt_ref;
 
 		// reinitialize map projection to latitude, longitude, altitude, and reset position
 		_pos_ref.initReference(latitude, longitude, _time_delayed_us);
@@ -114,30 +114,24 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
 
 		_NED_origin_initialised = true;
 
-		// minimum change in position or height that triggers a reset
-		static constexpr float MIN_RESET_DIST_M = 0.01f;
-
 		if (current_pos_available) {
-			// reset horizontal position
+			// reset horizontal position if we already have a global origin
 			Vector2f position = _pos_ref.project(current_lat, current_lon);
-
-			if (Vector2f(position - Vector2f(_state.pos)).longerThan(MIN_RESET_DIST_M)) {
-				resetHorizontalPositionTo(position);
-			}
+			resetHorizontalPositionTo(position);
 		}
 
-		// reset vertical position (if there's any change)
-		if (fabsf(altitude - gps_alt_ref_prev) > MIN_RESET_DIST_M) {
+		if (PX4_ISFINITE(gps_alt_ref_prev) && isVerticalPositionAidingActive()) {
 			// determine current z
-			float current_alt = -_state.pos(2) + gps_alt_ref_prev;
-
+			const float z_prev = _state.pos(2);
+			const float current_alt = -z_prev + gps_alt_ref_prev;
 #if defined(CONFIG_EKF2_GNSS)
 			const float gps_hgt_bias = _gps_hgt_b_est.getBias();
 #endif // CONFIG_EKF2_GNSS
 			resetVerticalPositionTo(_gps_alt_ref - current_alt);
-
+			ECL_DEBUG("EKF global origin updated, resetting vertical position %.1fm -> %.1fm", (double)z_prev,
+				  (double)_state.pos(2));
 #if defined(CONFIG_EKF2_GNSS)
-			// preserve GPS height bias
+			// adjust existing GPS height bias
 			_gps_hgt_b_est.setBias(gps_hgt_bias);
 #endif // CONFIG_EKF2_GNSS
 		}
@@ -574,44 +568,91 @@ void Ekf::updateDeadReckoningStatus()
 
 void Ekf::updateHorizontalDeadReckoningstatus()
 {
-	const bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.aux_gpos)
-				  && (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max)
-				      || isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max));
+	bool inertial_dead_reckoning = true;
+	bool aiding_expected_in_air = false;
+
+	// velocity aiding active
+	if ((_control_status.flags.gps || _control_status.flags.ev_vel)
+	&& isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max)
+	) {
+		inertial_dead_reckoning = false;
+	}
+
+	// position aiding active
+	if ((_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.aux_gpos)
+	&& isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max)
+	) {
+		inertial_dead_reckoning = false;
+	}
 
-	bool optFlowAiding = false;
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	optFlowAiding = _control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max);
+	// optical flow active
+	if (_control_status.flags.opt_flow
+	&& isRecent(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max)
+	) {
+		inertial_dead_reckoning = false;
+
+	} else {
+		if (!_control_status.flags.in_air && (_params.flow_ctrl == 1)
+		&& isRecent(_aid_src_optical_flow.timestamp_sample, _params.no_aid_timeout_max)
+		) {
+			// currently landed, but optical flow aiding should be possible once in air
+			aiding_expected_in_air = true;
+		}
+	}
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
-	bool airDataAiding = false;
-
 #if defined(CONFIG_EKF2_AIRSPEED)
-	airDataAiding = _control_status.flags.wind &&
-				   isRecent(_aid_src_airspeed.time_last_fuse, _params.no_aid_timeout_max) &&
-				   isRecent(_aid_src_sideslip.time_last_fuse, _params.no_aid_timeout_max);
+	// air data aiding active
+	if ((_control_status.flags.fuse_aspd && isRecent(_aid_src_airspeed.time_last_fuse, _params.no_aid_timeout_max))
+	&& (_control_status.flags.fuse_beta && isRecent(_aid_src_sideslip.time_last_fuse, _params.no_aid_timeout_max))
+	) {
+		// wind_dead_reckoning: no other aiding but air data
+		_control_status.flags.wind_dead_reckoning = inertial_dead_reckoning;
 
-	_control_status.flags.wind_dead_reckoning = !velPosAiding && !optFlowAiding && airDataAiding;
-#else
-	_control_status.flags.wind_dead_reckoning = false;
+		// air data aiding is active, we're not inertial dead reckoning
+		inertial_dead_reckoning = false;
+
+	} else {
+		_control_status.flags.wind_dead_reckoning = false;
+
+		if (!_control_status.flags.in_air && _control_status.flags.fixed_wing
+		&& (_params.beta_fusion_enabled == 1)
+		&& (_params.arsp_thr > 0.f) && isRecent(_aid_src_airspeed.timestamp_sample, _params.no_aid_timeout_max)
+		) {
+			// currently landed, but air data aiding should be possible once in air
+			aiding_expected_in_air = true;
+		}
+	}
 #endif // CONFIG_EKF2_AIRSPEED
 
-	_control_status.flags.inertial_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding;
-
-	if (!_control_status.flags.inertial_dead_reckoning) {
-		if (_time_delayed_us > _params.no_aid_timeout_max) {
-			_time_last_horizontal_aiding = _time_delayed_us - _params.no_aid_timeout_max;
+	// zero velocity update
+	if (isRecent(_zero_velocity_update.time_last_fuse(), _params.no_aid_timeout_max)) {
+		// only respect as a valid aiding source now if we expect to have another valid source once in air
+		if (aiding_expected_in_air) {
+			inertial_dead_reckoning = false;
 		}
 	}
 
-	// report if we have been deadreckoning for too long, initial state is deadreckoning until aiding is present
-	bool deadreckon_time_exceeded = isTimedOut(_time_last_horizontal_aiding, (uint64_t)_params.valid_timeout_max);
+	if (inertial_dead_reckoning) {
+		if (isTimedOut(_time_last_horizontal_aiding, (uint64_t)_params.valid_timeout_max)) {
+			// deadreckon time exceeded
+			if (!_horizontal_deadreckon_time_exceeded) {
+				ECL_WARN("horizontal dead reckon time exceeded");
+				_horizontal_deadreckon_time_exceeded = true;
+			}
+		}
+
+	} else {
+		if (_time_delayed_us > _params.no_aid_timeout_max) {
+			_time_last_horizontal_aiding = _time_delayed_us - _params.no_aid_timeout_max;
+		}
+
+		_horizontal_deadreckon_time_exceeded = false;
 
-	if (!_horizontal_deadreckon_time_exceeded && deadreckon_time_exceeded) {
-		// deadreckon time now exceeded
-		ECL_WARN("dead reckon time exceeded");
 	}
 
-	_horizontal_deadreckon_time_exceeded = deadreckon_time_exceeded;
+	_control_status.flags.inertial_dead_reckoning = inertial_dead_reckoning;
 }
 
 void Ekf::updateVerticalDeadReckoningStatus()

src/modules/ekf2/EKF2.cpp

@@ -497,6 +497,12 @@ void EKF2::Run()
 		vehicle_command_s vehicle_command;
 
 		if (_vehicle_command_sub.update(&vehicle_command)) {
+
+			vehicle_command_ack_s command_ack{};
+			command_ack.command = vehicle_command.command;
+			command_ack.target_system = vehicle_command.source_system;
+			command_ack.target_component = vehicle_command.source_component;
+
 			if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN) {
 				double latitude = vehicle_command.param5;
 				double longitude = vehicle_command.param6;
@@ -509,15 +515,23 @@ void EKF2::Run()
 					PX4_INFO("%d - New NED origin (LLA): %3.10f, %3.10f, %4.3f\n",
 						 _instance, latitude, longitude, static_cast<double>(altitude));
 
+					command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+
 				} else {
 					PX4_ERR("%d - Failed to set new NED origin (LLA): %3.10f, %3.10f, %4.3f\n",
 						_instance, latitude, longitude, static_cast<double>(altitude));
-				}
-			}
 
-			if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE) {
+					command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED;
+				}
+
+				command_ack.timestamp = hrt_absolute_time();
+				_vehicle_command_ack_pub.publish(command_ack);
+
+			} else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE) {
+
 				if ((_ekf.control_status_flags().wind_dead_reckoning || _ekf.control_status_flags().inertial_dead_reckoning) &&
-				    PX4_ISFINITE(vehicle_command.param2) && PX4_ISFINITE(vehicle_command.param5) && PX4_ISFINITE(vehicle_command.param6)) {
+				    PX4_ISFINITE(vehicle_command.param2) && PX4_ISFINITE(vehicle_command.param5) && PX4_ISFINITE(vehicle_command.param6)
+				   ) {
 
 					const float measurement_delay_seconds = math::constrain(vehicle_command.param2, 0.0f,
 										kMaxDelaySecondsExternalPosMeasurement);
@@ -530,9 +544,21 @@ void EKF2::Run()
 					}
 
 					// TODO add check for lat and long validity
-					_ekf.resetGlobalPosToExternalObservation(vehicle_command.param5, vehicle_command.param6,
-							accuracy, timestamp_observation);
+					if (_ekf.resetGlobalPosToExternalObservation(vehicle_command.param5, vehicle_command.param6,
+							accuracy, timestamp_observation)
+					   ) {
+						command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+
+					} else {
+						command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED;
+					}
+
+				} else {
+					command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; // TODO: expand
 				}
+
+				command_ack.timestamp = hrt_absolute_time();
+				_vehicle_command_ack_pub.publish(command_ack);
 			}
 		}
 	}

src/modules/ekf2/EKF2.hpp

@@ -78,6 +78,7 @@
 #include <uORB/topics/sensor_selection.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_imu.h>
 #include <uORB/topics/vehicle_land_detected.h>
@@ -387,9 +388,11 @@ private:
 
 	uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
 	uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
-	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 
+	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
+	uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
+
 	uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)};
 	uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)};
 

src/modules/ekf2/test/test_EKF_gps.cpp

@@ -105,7 +105,7 @@ TEST_F(EkfGpsTest, gpsFixLoss)
 	_sensor_simulator._gps.setFixType(0);
 
 	// THEN: after dead-reconing for a couple of seconds, the local position gets invalidated
-	_sensor_simulator.runSeconds(5);
+	_sensor_simulator.runSeconds(6);
 	EXPECT_TRUE(_ekf->control_status_flags().inertial_dead_reckoning);
 	EXPECT_FALSE(_ekf->local_position_is_valid());