added support to reset vehicle position based on external position (#22444)

via command MAV_CMD_EXTERNAL_POSITION_ESTIMATE

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/modules/ekf2/EKF/common.h

@@ -655,6 +655,7 @@ union information_event_status_u {
 		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
+		bool reset_pos_to_ext_obs       : 1; ///< 17 - true when horizontal position was reset to an external observation while deadreckoning
 	} flags;
 	uint32_t value;
 };

src/modules/ekf2/EKF/ekf.cpp

@@ -341,6 +341,22 @@ 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) {
+
+   if (!_pos_ref.isInitialized()) {
+       return;
+   }
+
+   // 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) + _state.vel.xy() * dt;
+
+    resetHorizontalPositionToExternal(pos_corrected, math::max(accuracy, FLT_EPSILON));
+}
+
 void Ekf::updateParameters()
 {
 #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)

src/modules/ekf2/EKF/ekf.h

@@ -502,6 +502,8 @@ public:
 		return is_healthy;
 	}
 
+    void resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation);
+
 	void updateParameters();
 
 	friend class AuxGlobalPosition;
@@ -812,6 +814,7 @@ private:
 
 	void resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var);
 	void resetHorizontalPositionToLastKnown();
+    void resetHorizontalPositionToExternal(const Vector2f &new_horiz_pos, float horiz_accuracy);
 
 	void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var);
 	void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const float pos_var = NAN) { resetHorizontalPositionTo(new_horz_pos, Vector2f(pos_var, pos_var)); }

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -171,6 +171,12 @@ bool Ekf::isHeightResetRequired() const
 	return (continuous_bad_accel_hgt || hgt_fusion_timeout);
 }
 
+void Ekf::resetHorizontalPositionToExternal(const Vector2f &new_horiz_pos, float horiz_accuracy) {
+    _information_events.flags.reset_pos_to_ext_obs = true;
+    ECL_INFO("reset position to external observation");
+    resetHorizontalPositionTo(new_horiz_pos, sq(horiz_accuracy));
+}
+
 void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_var)
 {
 	const float old_vert_pos = _state.pos(2);