diff --git a/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp index 97f7c32fe391..58231627f329 100644 --- a/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp @@ -66,9 +66,10 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) _external_wind_init = false; } -#if defined(CONFIG_EKF2_GNSS) + const bool fixed_wing_or_transition = _control_status.flags.fixed_wing || _control_status.flags.in_transition; - // clear yaw estimator airspeed (updated later with true airspeed if airspeed fusion is active) +#if defined(CONFIG_EKF2_GNSS) + // EKF-GSF airspeed compensation assumes fixed-wing dynamics. if (_control_status.flags.fixed_wing) { if (_control_status.flags.in_air && !_control_status.flags.vehicle_at_rest) { if (!_control_status.flags.fuse_aspd) { @@ -78,6 +79,9 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) } else { _yawEstimator.setTrueAirspeed(0.f); } + + } else if (!_control_status.flags.in_transition) { + _yawEstimator.setTrueAirspeed(NAN); } #endif // CONFIG_EKF2_GNSS @@ -95,8 +99,13 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) updateAirspeed(airspeed_sample, _aid_src_airspeed); - const bool continuing_conditions_passing = _control_status.flags.in_air && (_control_status.flags.fixed_wing - || _control_status.flags.in_transition) + // Use lateral specific force to check the zero-sideslip assumption in MC mode. + const bool mc_airflow_aligned = (_params.ekf2_aspd_mc_lim <= 0.f) + || (fabsf(_aspd_mc_lat_accel_lpf.getState()) < _params.ekf2_aspd_mc_lim); + const bool mc_aspd_allowed = (_params.ekf2_aspd_mc == 1) && mc_airflow_aligned; + + const bool continuing_conditions_passing = _control_status.flags.in_air + && (fixed_wing_or_transition || mc_aspd_allowed) && !_control_status.flags.fake_pos; const bool is_airspeed_significant = airspeed_sample.true_airspeed > _params.ekf2_arsp_thr; @@ -112,7 +121,11 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) } #if defined(CONFIG_EKF2_GNSS) - _yawEstimator.setTrueAirspeed(airspeed_sample.true_airspeed); + + if (fixed_wing_or_transition) { + _yawEstimator.setTrueAirspeed(airspeed_sample.true_airspeed); + } + #endif // CONFIG_EKF2_GNSS const bool is_fusion_failing = isTimedOut(_aid_src_airspeed.time_last_fuse, (uint64_t)10e6); @@ -126,8 +139,10 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) } } else if (starting_conditions_passing) { - const bool do_vel_reset = _horizontal_deadreckon_time_exceeded - || (_control_status.flags.inertial_dead_reckoning && !is_airspeed_consistent); + // Never reset navigation states from airspeed in MC mode. + const bool do_vel_reset = (_horizontal_deadreckon_time_exceeded + || (_control_status.flags.inertial_dead_reckoning && !is_airspeed_consistent)) + && fixed_wing_or_transition; const Vector2f wind_vel_var = getWindVelocityVariance(); const bool do_wind_reset = (!_control_status.flags.wind || ((wind_vel_var(0) + wind_vel_var(1)) > sq(_params.initial_wind_uncertainty))) @@ -192,8 +207,9 @@ void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_sour return; } - // determine if we need the airspeed fusion to correct states other than wind - const bool update_wind_only = !_control_status.flags.wind_dead_reckoning; + // Airspeed never corrects navigation states in MC mode. + const bool update_wind_only = !_control_status.flags.wind_dead_reckoning + || !(_control_status.flags.fixed_wing || _control_status.flags.in_transition); const float innov_var = aid_src.innovation_variance; diff --git a/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp index 48e5cb767096..0c94f4827d8d 100644 --- a/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp @@ -97,8 +97,9 @@ bool Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) return false; } - // determine if we need the sideslip fusion to correct states other than wind - bool update_wind_only = !_control_status.flags.wind_dead_reckoning; + // Synthetic sideslip never corrects navigation states in MC mode. + const bool update_wind_only = !_control_status.flags.wind_dead_reckoning + || !(_control_status.flags.fixed_wing || _control_status.flags.in_transition); // Reset covariance and states if the calculation is badly conditioned if ((sideslip.innovation_variance < sideslip.observation_variance) diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index c47d317282c2..365f53743ea5 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -424,6 +424,8 @@ struct parameters { float ekf2_tas_gate{5.0f}; ///< True Airspeed innovation consistency gate size (STD) float ekf2_eas_noise{1.4f}; ///< EAS measurement noise standard deviation used for airspeed fusion (m/s) float ekf2_arsp_thr{2.0f}; ///< Airspeed fusion threshold. A value of zero will deactivate airspeed fusion + int32_t ekf2_aspd_mc{0}; ///< Enable airspeed fusion for wind estimation in MC mode + float ekf2_aspd_mc_lim{1.0f}; ///< MC airspeed fusion lateral specific force limit (m/sec**2); 0 disables #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_SIDESLIP) diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 74f4a3b5a886..8d4a3bc55f26 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -275,6 +275,10 @@ void Ekf::predictState(const imuSample &imu_delayed) // calculate a filtered horizontal acceleration this are used for manoeuvre detection elsewhere _accel_horiz_lpf.update(corrected_delta_vel_ef.xy() / imu_delayed.delta_vel_dt, imu_delayed.delta_vel_dt); + +#if defined(CONFIG_EKF2_AIRSPEED) + _aspd_mc_lat_accel_lpf.update(corrected_delta_vel(1) / imu_delayed.delta_vel_dt, imu_delayed.delta_vel_dt); +#endif // CONFIG_EKF2_AIRSPEED } bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const double longitude, const float altitude, diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index dc00395c203d..2fb2f9e2aaf9 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -550,6 +550,11 @@ class Ekf final : public EstimatorInterface static constexpr float _kAccelHorizLpfTimeConstant = 1.f; AlphaFilter _accel_horiz_lpf{_kAccelHorizLpfTimeConstant}; ///< Low pass filtered horizontal earth frame acceleration (m/sec**2) +#if defined(CONFIG_EKF2_AIRSPEED) + static constexpr float _kAspdMcLatAccelLpfTimeConstant = 1.f; + AlphaFilter _aspd_mc_lat_accel_lpf{_kAspdMcLatAccelLpfTimeConstant}; ///< Filtered body-Y specific force (m/sec**2) +#endif // CONFIG_EKF2_AIRSPEED + #if defined(CONFIG_EKF2_WIND) static constexpr float _kHeightRateLpfTimeConstant = 10.f; AlphaFilter _height_rate_lpf{_kHeightRateLpfTimeConstant}; diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index c70d1a480652..548e782cde4b 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -842,8 +842,9 @@ void Ekf::updateHorizontalDeadReckoningstatus() #if defined(CONFIG_EKF2_AIRSPEED) - // air data aiding active - if ((_control_status.flags.fuse_aspd && isRecent(_aid_src_airspeed.time_last_fuse, _params.no_aid_timeout_max)) + // MC air data cannot provide dead-reckoning aiding. + if ((_control_status.flags.fixed_wing || _control_status.flags.in_transition) + && (_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 diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index a7841e5b5fd6..7c8477e1a05b 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -118,6 +118,8 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_tas_gate(_params->ekf2_tas_gate), _param_ekf2_eas_noise(_params->ekf2_eas_noise), _param_ekf2_arsp_thr(_params->ekf2_arsp_thr), + _param_ekf2_aspd_mc(_params->ekf2_aspd_mc), + _param_ekf2_aspd_mc_lim(_params->ekf2_aspd_mc_lim), #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_SIDESLIP) _param_ekf2_beta_gate(_params->ekf2_beta_gate), diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index fffed51789c7..874839ad12fc 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -610,6 +610,10 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem // control of airspeed fusion (ParamExtFloat) _param_ekf2_arsp_thr, ///< A value of zero will disabled airspeed fusion. Any positive value sets the minimum airspeed which will be used (m/sec) + (ParamExtInt) + _param_ekf2_aspd_mc, ///< Airspeed fusion for wind estimation in MC mode + (ParamExtFloat) + _param_ekf2_aspd_mc_lim, ///< MC airspeed fusion lateral specific force limit (m/s^2) #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_SIDESLIP) diff --git a/src/modules/ekf2/params_airspeed.yaml b/src/modules/ekf2/params_airspeed.yaml index b9563336a47a..de3daddde955 100644 --- a/src/modules/ekf2/params_airspeed.yaml +++ b/src/modules/ekf2/params_airspeed.yaml @@ -8,12 +8,33 @@ parameters: long: Airspeed data is fused for wind estimation if above this threshold. Set to 0 to disable airspeed fusion. For reliable wind estimation both sideslip (see EKF2_FUSE_BETA) and airspeed fusion should be enabled. Only applies - to fixed-wing vehicles (or VTOLs in fixed-wing mode). + to fixed-wing vehicles (or VTOLs in fixed-wing mode), unless EKF2_ASPD_MC + is enabled. type: float default: 0.0 min: 0.0 unit: m/s decimal: 1 + EKF2_ASPD_MC: + description: + short: Airspeed fusion in multicopter mode + long: Allow true airspeed fusion for wind estimation in multicopter mode. + Only the wind states are corrected. Fusion stops when lateral specific force + exceeds EKF2_ASPD_MC_LIM. + type: boolean + default: 0 + EKF2_ASPD_MC_LIM: + description: + short: Multicopter lateral specific force limit + long: Maximum low-pass filtered body-Y specific force for airspeed fusion in + multicopter mode. Fusion stops above this value because the airspeed and + sideslip models assume zero sideslip. Set to 0 to disable the check. + type: float + default: 1.0 + min: 0.0 + max: 5.0 + unit: m/s^2 + decimal: 2 EKF2_ASP_DELAY: description: short: Airspeed measurement delay relative to IMU measurements diff --git a/src/modules/ekf2/test/sensor_simulator/airspeed.h b/src/modules/ekf2/test/sensor_simulator/airspeed.h index 261aa4fefc2c..692f767f9a56 100644 --- a/src/modules/ekf2/test/sensor_simulator/airspeed.h +++ b/src/modules/ekf2/test/sensor_simulator/airspeed.h @@ -51,7 +51,7 @@ class Airspeed: public Sensor Airspeed(std::shared_ptr ekf); ~Airspeed(); - void setData(float true_airspeed, float eas2tas); + void setData(float true_airspeed, float indicated_airspeed); private: float _true_airspeed_data{0.0f}; diff --git a/src/modules/ekf2/test/test_EKF_airspeed.cpp b/src/modules/ekf2/test/test_EKF_airspeed.cpp index 20d323160d53..6bea7c059c53 100644 --- a/src/modules/ekf2/test/test_EKF_airspeed.cpp +++ b/src/modules/ekf2/test/test_EKF_airspeed.cpp @@ -75,6 +75,32 @@ class EkfAirspeedTest : public ::testing::Test void TearDown() override { } + + void startExternalVisionVelocity(const Vector3f &velocity) + { + _ekf_wrapper.enableExternalVisionVelocityFusion(); + _sensor_simulator._vio.setVelocity(velocity); + _sensor_simulator._vio.setVelocityFrameToLocalNED(); + _sensor_simulator.startExternalVision(); + + // Allow EV fusion to reset velocity. + _sensor_simulator.runSeconds(0.5); + } + + void startMulticopterAirspeedFusionScenario(bool mc_airspeed_enabled, float true_airspeed) + { + parameters *params = _ekf->getParamHandle(); + params->ekf2_arsp_thr = 2.0f; + params->ekf2_aspd_mc = mc_airspeed_enabled ? 1 : 0; + params->ekf2_aspd_mc_lim = 1.0f; + _ekf_wrapper.enableBetaFusion(); + + _ekf->set_in_air_status(true); + _ekf->set_vehicle_at_rest(false); + _ekf->set_is_fixed_wing(false); + _sensor_simulator.startAirspeedSensor(); + _sensor_simulator._airspeed.setData(true_airspeed, true_airspeed); + } }; TEST_F(EkfAirspeedTest, testWindVelocityEstimation) @@ -346,3 +372,79 @@ TEST_F(EkfAirspeedTest, testExternalWindResetOnGround) EXPECT_TRUE(_ekf_wrapper.isIntendingBetaFusion()); EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); } + +TEST_F(EkfAirspeedTest, testMulticopterAirspeedFusionRequiresParam) +{ + startExternalVisionVelocity(Vector3f(0.0f, 1.5f, 0.0f)); + startMulticopterAirspeedFusionScenario(false, 2.4f); + + _sensor_simulator.runSeconds(2); + + EXPECT_FALSE(_ekf_wrapper.isIntendingAirspeedFusion()); + EXPECT_FALSE(_ekf_wrapper.isWindVelocityEstimated()); +} + +TEST_F(EkfAirspeedTest, testMulticopterWindVelocityEstimation) +{ + const Vector3f simulated_velocity_earth(0.0f, 1.5f, 0.0f); + const Vector2f airspeed_body(2.4f, 0.0f); + startExternalVisionVelocity(simulated_velocity_earth); + startMulticopterAirspeedFusionScenario(true, airspeed_body(0)); + + _sensor_simulator.runSeconds(15); + + EXPECT_TRUE(_ekf_wrapper.isIntendingAirspeedFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingBetaFusion()); + EXPECT_TRUE(_ekf_wrapper.isWindVelocityEstimated()); + + const Dcmf R_to_earth_sim(_quat_sim); + const Vector2f vel_wind_earth = _ekf->getWindVelocity(); + const Vector3f vel_wind_expected = simulated_velocity_earth - R_to_earth_sim * Vector3f(airspeed_body(0), + airspeed_body(1), 0.0f); + + EXPECT_NEAR(vel_wind_earth(0), vel_wind_expected(0), 1e-1f); + EXPECT_NEAR(vel_wind_earth(1), vel_wind_expected(1), 1e-1f); +} + +TEST_F(EkfAirspeedTest, testMulticopterAirspeedFusionStopsWhenMisaligned) +{ + startExternalVisionVelocity(Vector3f(0.0f, 1.5f, 0.0f)); + startMulticopterAirspeedFusionScenario(true, 2.4f); + + _sensor_simulator.runSeconds(2); + EXPECT_TRUE(_ekf_wrapper.isIntendingAirspeedFusion()); + + // Exceed the lateral specific force limit. + _sensor_simulator._imu.setAccelData(Vector3f(0.0f, 3.0f, -CONSTANTS_ONE_G)); + _sensor_simulator.runSeconds(2); + + EXPECT_FALSE(_ekf_wrapper.isIntendingAirspeedFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingBetaFusion()); + + // Fusion restarts once the specific force drops below the limit. + _sensor_simulator._imu.setAccelData(Vector3f(0.0f, 0.0f, -CONSTANTS_ONE_G)); + _sensor_simulator.runSeconds(5); + + EXPECT_TRUE(_ekf_wrapper.isIntendingAirspeedFusion()); + EXPECT_TRUE(_ekf_wrapper.isWindVelocityEstimated()); +} + +TEST_F(EkfAirspeedTest, testMulticopterAirspeedNoDeadReckoning) +{ + startExternalVisionVelocity(Vector3f(0.0f, 1.5f, 0.0f)); + startMulticopterAirspeedFusionScenario(true, 8.0f); + + _sensor_simulator.runSeconds(5); + EXPECT_TRUE(_ekf_wrapper.isIntendingAirspeedFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingBetaFusion()); + + // MC air data must not count as dead-reckoning aiding. + _sensor_simulator.stopExternalVision(); + _sensor_simulator.runSeconds(3); + + EXPECT_FALSE(_ekf->control_status_flags().wind_dead_reckoning); + EXPECT_TRUE(_ekf->control_status_flags().inertial_dead_reckoning); + + _sensor_simulator.runSeconds(5); + EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid()); +}