From e04c53241a44a60e8644da14dfc815d3c012d98f Mon Sep 17 00:00:00 2001 From: Marco Hauswirth <58551738+haumarco@users.noreply.github.com> Date: Sun, 7 Jul 2024 22:43:55 +0200 Subject: [PATCH] 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 --- .../init.d/airframes/4061_atl_mantis_edu | 1 + .../EKF/aid_sources/ZeroVelocityUpdate.cpp | 6 +- .../EKF/aid_sources/ZeroVelocityUpdate.hpp | 4 +- .../ekf2/EKF/aid_sources/fake_pos_control.cpp | 10 +- src/modules/ekf2/EKF/ekf.cpp | 63 ++++++++-- src/modules/ekf2/EKF/ekf.h | 2 +- src/modules/ekf2/EKF/ekf_helper.cpp | 119 ++++++++++++------ src/modules/ekf2/EKF2.cpp | 38 +++++- src/modules/ekf2/EKF2.hpp | 5 +- src/modules/ekf2/test/test_EKF_gps.cpp | 2 +- 10 files changed, 182 insertions(+), 68 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu index 27bd839775..66d0973caa 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu +++ b/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 # diff --git a/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp b/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp index 48d07d583d..052e37312c 100644 --- a/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp +++ b/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; } diff --git a/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.hpp b/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.hpp index 0016c584c6..a591d9dc96 100644 --- a/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.hpp +++ b/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) }; diff --git a/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp b/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp index 33f05ac68e..9ea6485e64 100644 --- a/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp @@ -75,15 +75,13 @@ void Ekf::controlFakePosFusion() Vector2f(getStateVariance()) + 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(ImuCtrl::GravityVector)) || _control_status.flags.vehicle_at_rest); - - const bool starting_conditions_passing = continuing_conditions_passing + && (!(_params.imu_ctrl & static_cast(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(); diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 1571aa06ca..983db8aa86 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/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(_time_delayed_us - timestamp_observation) - * 1e-6f : -static_cast(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(_time_delayed_us - timestamp_observation); + + } else { + diff_us = -static_cast(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()) + 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() diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index f7c4ee2dcf..966a45a650 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/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(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 94b1046dba..791ed89e26 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/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() diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 17d55ab740..362c68c372 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/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(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(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); } } } diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 2de08049cb..3bc2b99f95 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -78,6 +78,7 @@ #include #include #include +#include #include #include #include @@ -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_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)}; diff --git a/src/modules/ekf2/test/test_EKF_gps.cpp b/src/modules/ekf2/test/test_EKF_gps.cpp index 0e243176a2..b03a8701d5 100644 --- a/src/modules/ekf2/test/test_EKF_gps.cpp +++ b/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());