From bda58e96ff3170b5532c9837649efd91d8f030b0 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 17 Jun 2024 18:09:48 -0400 Subject: [PATCH] ekf2: move estimator_status test ratios to filtered values --- Tools/ecl_ekf/plotting/pdf_report.py | 2 +- .../scripts/itcm_functions_includes.ld | 1 - msg/EstimatorStatus.msg | 13 -- .../aux_global_position.cpp | 2 + .../aux_global_position.hpp | 4 + src/modules/ekf2/EKF/ekf.h | 19 +- src/modules/ekf2/EKF/ekf_helper.cpp | 179 +++++++++++++----- src/modules/ekf2/EKF2.cpp | 17 +- src/modules/mavlink/streams/HIGH_LATENCY2.hpp | 3 +- 9 files changed, 164 insertions(+), 76 deletions(-) diff --git a/Tools/ecl_ekf/plotting/pdf_report.py b/Tools/ecl_ekf/plotting/pdf_report.py index db5d4fdd508a..029589ba56ed 100644 --- a/Tools/ecl_ekf/plotting/pdf_report.py +++ b/Tools/ecl_ekf/plotting/pdf_report.py @@ -223,7 +223,7 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str data_plot.save() data_plot.close() - # plot innovation_check_flags summary + # plot innovation flags summary data_plot = CheckFlagsPlot( status_flags_time, estimator_status_flags, [['reject_hor_vel', 'reject_hor_pos'], ['reject_ver_vel', 'reject_ver_pos', 'reject_hagl'], diff --git a/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld b/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld index 477cc835bafb..2b32ba473ac3 100644 --- a/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld +++ b/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld @@ -154,7 +154,6 @@ *(.text._ZN3Ekf20updateIMUBiasInhibitERKN9estimator9imuSampleE) *(.text._ZN9Commander13dataLinkCheckEv) *(.text._ZN17FlightModeManager10switchTaskE15FlightTaskIndex) -*(.text._ZNK3Ekf26get_innovation_test_statusERtRfS1_S1_S1_S1_S1_S1_) *(.text._ZN12PX4Gyroscope9set_scaleEf) *(.text._ZN12FailsafeBase6updateERKyRKNS_5StateEbbRK16failsafe_flags_s) *(.text._ZN18MavlinkStreamDebug4sendEv) diff --git a/msg/EstimatorStatus.msg b/msg/EstimatorStatus.msg index 5491734b1d8e..62d044faaf80 100644 --- a/msg/EstimatorStatus.msg +++ b/msg/EstimatorStatus.msg @@ -69,19 +69,6 @@ uint32 filter_fault_flags # Bitmask to indicate EKF internal faults float32 pos_horiz_accuracy # 1-Sigma estimated horizontal position accuracy relative to the estimators origin (m) float32 pos_vert_accuracy # 1-Sigma estimated vertical position accuracy relative to the estimators origin (m) -uint16 innovation_check_flags # Bitmask to indicate pass/fail status of innovation consistency checks -# 0 - true if velocity observations have been rejected -# 1 - true if horizontal position observations have been rejected -# 2 - true if true if vertical position observations have been rejected -# 3 - true if the X magnetometer observation has been rejected -# 4 - true if the Y magnetometer observation has been rejected -# 5 - true if the Z magnetometer observation has been rejected -# 6 - true if the yaw observation has been rejected -# 7 - true if the airspeed observation has been rejected -# 8 - true if the synthetic sideslip observation has been rejected -# 9 - true if the height above ground observation has been rejected -# 10 - true if the X optical flow observation has been rejected -# 11 - true if the Y optical flow observation has been rejected float32 mag_test_ratio # ratio of the largest magnetometer innovation component to the innovation test limit float32 vel_test_ratio # ratio of the largest velocity innovation component to the innovation test limit diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp index 15be1c3ce6b4..fe313315b597 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp @@ -151,6 +151,8 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed #if defined(MODULE_NAME) aid_src.timestamp = hrt_absolute_time(); _estimator_aid_src_aux_global_position_pub.publish(aid_src); + + _test_ratio_filtered = math::max(fabsf(aid_src.test_ratio_filtered[0]), fabsf(aid_src.test_ratio_filtered[1])); #endif // MODULE_NAME } else if ((_state != State::stopped) && isTimedOut(_time_last_buffer_push, imu_delayed.time_us, (uint64_t)5e6)) { diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp index 056840c033c7..bd63f7245605 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp @@ -74,6 +74,8 @@ class AuxGlobalPosition : public ModuleParams updateParams(); } + float test_ratio_filtered() const { return _test_ratio_filtered; } + private: bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t time_delayed_us, uint64_t timeout_period) const { @@ -106,6 +108,8 @@ class AuxGlobalPosition : public ModuleParams State _state{State::stopped}; + float _test_ratio_filtered{INFINITY}; + #if defined(MODULE_NAME) struct reset_counters_s { uint8_t lat_lon{}; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index bf62b542f42c..463133354a76 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -383,13 +383,18 @@ class Ekf final : public EstimatorInterface *counter = _state_reset_status.reset_count.quat; } - // get EKF innovation consistency check status information comprising of: - // status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check - // Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold. - // A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF - // Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned. - void get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas, - float &hagl, float &beta) const; + float getHeadingInnovationTestRatio() const; + + float getHorizontalVelocityInnovationTestRatio() const; + float getVerticalVelocityInnovationTestRatio() const; + + float getHorizontalPositionInnovationTestRatio() const; + float getVerticalPositionInnovationTestRatio() const; + + float getAirspeedInnovationTestRatio() const; + float getSyntheticSideslipInnovationTestRatio() const; + + float getHeightAboveGroundInnovationTestRatio() const; // return a bitmask integer that describes which state estimates are valid void get_ekf_soln_status(uint16_t *status) const; diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 7b6b0b0cace7..d58a9f450e91 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -301,131 +301,224 @@ void Ekf::resetAccelBias() resetAccelBiasCov(); } -void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas, - float &hagl, float &beta) const +float Ekf::getHeadingInnovationTestRatio() const { - // return the integer bitmask containing the consistency check pass/fail status - status = _innov_check_fail_status.value; - - // return the largest magnetometer innovation test ratio - mag = 0.f; + // return the largest heading innovation test ratio + float test_ratio = -1.f; #if defined(CONFIG_EKF2_MAGNETOMETER) if (_control_status.flags.mag_hdg ||_control_status.flags.mag_3D) { - mag = math::max(mag, sqrtf(Vector3f(_aid_src_mag.test_ratio).max())); + for (auto &test_ratio_filtered : _aid_src_mag.test_ratio_filtered) { + test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered)); + } } #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_GNSS_YAW) if (_control_status.flags.gps_yaw) { - mag = math::max(mag, sqrtf(_aid_src_gnss_yaw.test_ratio)); + test_ratio = math::max(test_ratio, fabsf(_aid_src_gnss_yaw.test_ratio_filtered)); } #endif // CONFIG_EKF2_GNSS_YAW #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_yaw) { - mag = math::max(mag, sqrtf(_aid_src_ev_yaw.test_ratio)); + test_ratio = math::max(test_ratio, fabsf(_aid_src_ev_yaw.test_ratio_filtered)); } #endif // CONFIG_EKF2_EXTERNAL_VISION - // return the largest velocity and position innovation test ratio - vel = NAN; - pos = NAN; + if (test_ratio >= 0.f && test_ratio <= 1.f) { + return sqrtf(test_ratio); + } + + return INFINITY; +} + +float Ekf::getHorizontalVelocityInnovationTestRatio() const +{ + // return the largest velocity innovation test ratio + float test_ratio = -1.f; #if defined(CONFIG_EKF2_GNSS) if (_control_status.flags.gps) { - float gps_vel = sqrtf(Vector3f(_aid_src_gnss_vel.test_ratio).max()); - vel = math::max(gps_vel, FLT_MIN); + for (int i = 0; i < 2; i++) { + test_ratio = math::max(test_ratio, fabsf(_aid_src_gnss_vel.test_ratio_filtered[i])); + } + } +#endif // CONFIG_EKF2_GNSS + +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + if (_control_status.flags.ev_vel) { + for (int i = 0; i < 2; i++) { + test_ratio = math::max(test_ratio, fabsf(_aid_src_ev_vel.test_ratio_filtered[i])); + } + } +#endif // CONFIG_EKF2_EXTERNAL_VISION + +#if defined(CONFIG_EKF2_OPTICAL_FLOW) + if (isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)) { + for (auto &test_ratio_filtered : _aid_src_optical_flow.test_ratio_filtered) { + test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered)); + } + } +#endif // CONFIG_EKF2_OPTICAL_FLOW - float gps_pos = sqrtf(Vector2f(_aid_src_gnss_pos.test_ratio).max()); - pos = math::max(gps_pos, FLT_MIN); + if (test_ratio >= 0.f && test_ratio <= 1.f) { + return sqrtf(test_ratio); + } + + return INFINITY; +} + +float Ekf::getVerticalVelocityInnovationTestRatio() const +{ + // return the largest velocity innovation test ratio + float test_ratio = -1.f; + +#if defined(CONFIG_EKF2_GNSS) + if (_control_status.flags.gps) { + test_ratio = math::max(test_ratio, fabsf(_aid_src_gnss_vel.test_ratio_filtered[2])); } #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_vel) { - float ev_vel = sqrtf(Vector3f(_aid_src_ev_vel.test_ratio).max()); - vel = math::max(vel, ev_vel, FLT_MIN); + test_ratio = math::max(test_ratio, fabsf(_aid_src_ev_vel.test_ratio_filtered[2])); } +#endif // CONFIG_EKF2_EXTERNAL_VISION + if (test_ratio >= 0.f && test_ratio <= 1.f) { + return sqrtf(test_ratio); + } + + return INFINITY; +} + +float Ekf::getHorizontalPositionInnovationTestRatio() const +{ + // return the largest position innovation test ratio + float test_ratio = -1.f; + +#if defined(CONFIG_EKF2_GNSS) + if (_control_status.flags.gps) { + for (auto &test_ratio_filtered : _aid_src_gnss_pos.test_ratio_filtered) { + test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered)); + } + } +#endif // CONFIG_EKF2_GNSS + +#if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_pos) { - float ev_pos = sqrtf(Vector2f(_aid_src_ev_pos.test_ratio).max()); - pos = math::max(pos, ev_pos, FLT_MIN); + for (auto &test_ratio_filtered : _aid_src_ev_pos.test_ratio_filtered) { + test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered)); + } } #endif // CONFIG_EKF2_EXTERNAL_VISION -#if defined(CONFIG_EKF2_OPTICAL_FLOW) - if (isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)) { - float of_vel = sqrtf(Vector2f(_aid_src_optical_flow.test_ratio).max()); - vel = math::max(of_vel, FLT_MIN); +#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) + if (_control_status.flags.aux_gpos) { + test_ratio = math::max(test_ratio, fabsf(_aux_global_position.test_ratio_filtered())); } -#endif // CONFIG_EKF2_OPTICAL_FLOW +#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION + if (test_ratio >= 0.f && test_ratio <= 1.f) { + return sqrtf(test_ratio); + } + + return INFINITY; +} + +float Ekf::getVerticalPositionInnovationTestRatio() const +{ // return the combined vertical position innovation test ratio float hgt_sum = 0.f; int n_hgt_sources = 0; #if defined(CONFIG_EKF2_BAROMETER) if (_control_status.flags.baro_hgt) { - hgt_sum += sqrtf(_aid_src_baro_hgt.test_ratio); + hgt_sum += sqrtf(fabsf(_aid_src_baro_hgt.test_ratio_filtered)); n_hgt_sources++; } #endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_GNSS) if (_control_status.flags.gps_hgt) { - hgt_sum += sqrtf(_aid_src_gnss_hgt.test_ratio); + hgt_sum += sqrtf(fabsf(_aid_src_gnss_hgt.test_ratio_filtered)); n_hgt_sources++; } #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_RANGE_FINDER) if (_control_status.flags.rng_hgt) { - hgt_sum += sqrtf(_aid_src_rng_hgt.test_ratio); + hgt_sum += sqrtf(fabsf(_aid_src_rng_hgt.test_ratio_filtered)); n_hgt_sources++; } #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_EXTERNAL_VISION) if (_control_status.flags.ev_hgt) { - hgt_sum += sqrtf(_aid_src_ev_hgt.test_ratio); + hgt_sum += sqrtf(fabsf(_aid_src_ev_hgt.test_ratio_filtered)); n_hgt_sources++; } #endif // CONFIG_EKF2_EXTERNAL_VISION if (n_hgt_sources > 0) { - hgt = math::max(hgt_sum / static_cast(n_hgt_sources), FLT_MIN); - - } else { - hgt = NAN; + return math::max(hgt_sum / static_cast(n_hgt_sources), FLT_MIN); } + return INFINITY; +} + +float Ekf::getAirspeedInnovationTestRatio() const +{ #if defined(CONFIG_EKF2_AIRSPEED) - // return the airspeed fusion innovation test ratio - tas = sqrtf(_aid_src_airspeed.test_ratio); + if (_control_status.flags.fuse_aspd) { + // return the airspeed fusion innovation test ratio + return sqrtf(fabsf(_aid_src_airspeed.test_ratio_filtered)); + } #endif // CONFIG_EKF2_AIRSPEED - hagl = NAN; + return INFINITY; +} + +float Ekf::getSyntheticSideslipInnovationTestRatio() const +{ +#if defined(CONFIG_EKF2_SIDESLIP) + if (_control_status.flags.fuse_beta) { + // return the synthetic sideslip innovation test ratio + return sqrtf(fabsf(_aid_src_sideslip.test_ratio_filtered)); + } +#endif // CONFIG_EKF2_SIDESLIP + + return INFINITY; +} + +float Ekf::getHeightAboveGroundInnovationTestRatio() const +{ + float test_ratio = -1.f; + #if defined(CONFIG_EKF2_TERRAIN) # if defined(CONFIG_EKF2_RANGE_FINDER) if (_hagl_sensor_status.flags.range_finder) { // return the terrain height innovation test ratio - hagl = sqrtf(_aid_src_terrain_range_finder.test_ratio); + test_ratio = math::max(test_ratio, fabsf(_aid_src_terrain_range_finder.test_ratio_filtered)); } #endif // CONFIG_EKF2_RANGE_FINDER # if defined(CONFIG_EKF2_OPTICAL_FLOW) if (_hagl_sensor_status.flags.flow) { // return the terrain height innovation test ratio - hagl = sqrtf(math::max(_aid_src_terrain_optical_flow.test_ratio[0], _aid_src_terrain_optical_flow.test_ratio[1])); + for (auto &test_ratio_filtered : _aid_src_terrain_optical_flow.test_ratio_filtered) { + test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered)); + } } # endif // CONFIG_EKF2_OPTICAL_FLOW #endif // CONFIG_EKF2_TERRAIN -#if defined(CONFIG_EKF2_SIDESLIP) - // return the synthetic sideslip innovation test ratio - beta = sqrtf(_aid_src_sideslip.test_ratio); -#endif // CONFIG_EKF2_SIDESLIP + if (test_ratio >= 0.f && test_ratio <= 1.f) { + return sqrtf(test_ratio); + } + + return INFINITY; } void Ekf::get_ekf_soln_status(uint16_t *status) const diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 2d15b7262cc9..b71b9675aaa6 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1802,15 +1802,14 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp) status.control_mode_flags = _ekf.control_status().value; status.filter_fault_flags = _ekf.fault_status().value; - uint16_t innov_check_flags_temp = 0; - _ekf.get_innovation_test_status(innov_check_flags_temp, status.mag_test_ratio, - status.vel_test_ratio, status.pos_test_ratio, - status.hgt_test_ratio, status.tas_test_ratio, - status.hagl_test_ratio, status.beta_test_ratio); - - // Bit mismatch between ecl and Firmware, combine the 2 first bits to preserve msg definition - // TODO: legacy use only, those flags are also in estimator_status_flags - status.innovation_check_flags = (innov_check_flags_temp >> 1) | (innov_check_flags_temp & 0x1); + status.mag_test_ratio = _ekf.getHeadingInnovationTestRatio(); + status.vel_test_ratio = math::max(_ekf.getHorizontalVelocityInnovationTestRatio(), + _ekf.getVerticalVelocityInnovationTestRatio()); + status.pos_test_ratio = _ekf.getHorizontalPositionInnovationTestRatio(); + status.hgt_test_ratio = _ekf.getVerticalPositionInnovationTestRatio(); + status.tas_test_ratio = _ekf.getAirspeedInnovationTestRatio(); + status.hagl_test_ratio = _ekf.getHeightAboveGroundInnovationTestRatio(); + status.beta_test_ratio = _ekf.getSyntheticSideslipInnovationTestRatio(); _ekf.get_ekf_lpos_accuracy(&status.pos_horiz_accuracy, &status.pos_vert_accuracy); _ekf.get_ekf_soln_status(&status.solution_status_flags); diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index 6ce85a2e7583..32d244b138da 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -309,8 +309,7 @@ class MavlinkStreamHighLatency2 : public MavlinkStream if (_estimator_status_sub.update(&estimator_status)) { if (estimator_status.gps_check_fail_flags > 0 || - estimator_status.filter_fault_flags > 0 || - estimator_status.innovation_check_flags > 0) { + estimator_status.filter_fault_flags > 0) { msg->failure_flags |= HL_FAILURE_FLAG_ESTIMATOR; }