From 2fd87c47e82706d8567458a96e0d5f26413488c7 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 1 Jul 2022 14:38:29 +0200 Subject: [PATCH] ekf2: use estimator_aid_source_1d message for airspeed fusion split the fusion process into: 1. updateAirspeed: computes innov, innov_var, obs_var, ... 2. fuseAirspeed: uses data computed in 1. to generate K, H and fuse the observation --- msg/estimator_aid_source_1d.msg | 2 +- src/modules/ekf2/EKF/airspeed_fusion.cpp | 131 ++++++++++++--------- src/modules/ekf2/EKF/control.cpp | 10 +- src/modules/ekf2/EKF/ekf.h | 15 +-- src/modules/ekf2/EKF/ekf_helper.cpp | 4 +- src/modules/ekf2/EKF/estimator_interface.h | 1 - 6 files changed, 90 insertions(+), 73 deletions(-) diff --git a/msg/estimator_aid_source_1d.msg b/msg/estimator_aid_source_1d.msg index c0f6695fd4..d08c2bb2f9 100644 --- a/msg/estimator_aid_source_1d.msg +++ b/msg/estimator_aid_source_1d.msg @@ -19,4 +19,4 @@ bool innovation_rejected # true if the observation has been rejected bool fused # true if the sample was successfully fused # TOPICS estimator_aid_source_1d -# TOPICS estimator_aid_src_baro_hgt estimator_aid_src_rng_hgt +# TOPICS estimator_aid_src_baro_hgt estimator_aid_src_rng_hgt estimator_aid_src_airspeed diff --git a/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/airspeed_fusion.cpp index 4b7a38797b..18749224e8 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/airspeed_fusion.cpp @@ -45,8 +45,11 @@ #include "ekf.h" #include -void Ekf::fuseAirspeed() +void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source_1d_s &airspeed) const { + // reset flags + resetEstimatorAidStatusFlags(airspeed); + const float vn = _state.vel(0); // Velocity in north direction const float ve = _state.vel(1); // Velocity in east direction const float vd = _state.vel(2); // Velocity in downwards direction @@ -55,7 +58,51 @@ void Ekf::fuseAirspeed() // Variance for true airspeed measurement - (m/sec)^2 const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * - math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f)); + math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f)); + + // Intermediate variables + const float IV0 = ve - vwe; + const float IV1 = vn - vwn; + const float IV2 = (IV0)*(IV0) + (IV1)*(IV1) + (vd)*(vd); + + const float predicted_airspeed = sqrtf(IV2); + + if (fabsf(predicted_airspeed) < FLT_EPSILON) { + return; + } + + const float IV3 = 1.0F/(IV2); + const float IV4 = IV0*P(5,23); + const float IV5 = IV0*IV3; + const float IV6 = IV1*P(4,22); + const float IV7 = IV1*IV3; + + const float innov_var = IV3*vd*(IV0*P(5,6) - IV0*P(6,23) + IV1*P(4,6) - IV1*P(6,22) + P(6,6)*vd) - IV5*(-IV0*P(23,23) - IV1*P(22,23) + IV1*P(4,23) + IV4 + P(6,23)*vd) + IV5*(IV0*P(5,5) + IV1*P(4,5) - IV1*P(5,22) - IV4 + P(5,6)*vd) - IV7*(-IV0*P(22,23) + IV0*P(5,22) - IV1*P(22,22) + IV6 + P(6,22)*vd) + IV7*(-IV0*P(4,23) + IV0*P(4,5) + IV1*P(4,4) - IV6 + P(4,6)*vd) + R_TAS; + + airspeed.observation = airspeed_sample.true_airspeed; + airspeed.observation_variance = R_TAS; + airspeed.innovation = predicted_airspeed - airspeed.observation; + airspeed.innovation_variance = innov_var; + + airspeed.fusion_enabled = _control_status.flags.fuse_aspd; + + airspeed.timestamp_sample = airspeed_sample.time_us; + + const float innov_gate = fmaxf(_params.tas_innov_gate, 1.f); + setEstimatorAidStatusTestRatio(airspeed, innov_gate); +} + +void Ekf::fuseAirspeed(estimator_aid_source_1d_s &airspeed) +{ + if (airspeed.innovation_rejected) { + return; + } + + const float vn = _state.vel(0); // Velocity in north direction + const float ve = _state.vel(1); // Velocity in east direction + const float vd = _state.vel(2); // Velocity in downwards direction + const float vwn = _state.wind_vel(0); // Wind speed in north direction + const float vwe = _state.wind_vel(1); // Wind speed in east direction // determine if we need the airspeed fusion to correct states other than wind const bool update_wind_only = !_control_status.flags.wind_dead_reckoning; @@ -63,34 +110,25 @@ void Ekf::fuseAirspeed() // Intermediate variables const float HK0 = vn - vwn; const float HK1 = ve - vwe; - const float HK2 = ecl::powf(HK0, 2) + ecl::powf(HK1, 2) + ecl::powf(vd, 2); - const float v_tas_pred = sqrtf(HK2); // predicted airspeed + const float HK2 = sqrtf((HK0)*(HK0) + (HK1)*(HK1) + (vd)*(vd)); - //const float HK3 = powf(HK2, -1.0F/2.0F); - if (v_tas_pred < 1.0f) { + const float predicted_airspeed = HK2; + + if (predicted_airspeed < 1.0f) { // calculation can be badly conditioned for very low airspeed values so don't fuse this time return; } - const float HK3 = 1.0f / v_tas_pred; + const float HK3 = 1.0F/(HK2); const float HK4 = HK0*HK3; const float HK5 = HK1*HK3; - const float HK6 = 1.0F/HK2; - const float HK7 = HK0*P(4,6) - HK0*P(6,22) + HK1*P(5,6) - HK1*P(6,23) + P(6,6)*vd; - const float HK8 = HK1*P(5,23); - const float HK9 = HK0*P(4,5) - HK0*P(5,22) + HK1*P(5,5) - HK8 + P(5,6)*vd; - const float HK10 = HK1*HK6; - const float HK11 = HK0*P(4,22); - const float HK12 = HK0*P(4,4) - HK1*P(4,23) + HK1*P(4,5) - HK11 + P(4,6)*vd; - const float HK13 = HK0*HK6; - const float HK14 = -HK0*P(22,23) + HK0*P(4,23) - HK1*P(23,23) + HK8 + P(6,23)*vd; - const float HK15 = -HK0*P(22,22) - HK1*P(22,23) + HK1*P(5,22) + HK11 + P(6,22)*vd; - //const float HK16 = HK3/(-HK10*HK14 + HK10*HK9 + HK12*HK13 - HK13*HK15 + HK6*HK7*vd + R_TAS); - - // innovation variance - check for badly conditioned calculation - _airspeed_innov_var = (-HK10 * HK14 + HK10 * HK9 + HK12 * HK13 - HK13 * HK15 + HK6 * HK7 * vd + R_TAS); - - if (_airspeed_innov_var < R_TAS) { // + const float HK6 = HK3*vd; + const float HK7 = -HK0*HK3; + const float HK8 = -HK1*HK3; + + const float innov_var = airspeed.innovation_variance; + + if (innov_var < airspeed.observation_variance || innov_var < FLT_EPSILON) { // Reset the estimator covariance matrix // if we are getting aiding from other sources, warn and reset the wind states and covariances only const char *action_string = nullptr; @@ -112,58 +150,37 @@ void Ekf::fuseAirspeed() return; } - const float HK16 = HK3 / _airspeed_innov_var; + const float HK9 = 1.0F/(innov_var); + _fault_status.flags.bad_airspeed = false; // Observation Jacobians SparseVector24f<4,5,6,22,23> Hfusion; Hfusion.at<4>() = HK4; Hfusion.at<5>() = HK5; - Hfusion.at<6>() = HK3*vd; - Hfusion.at<22>() = -HK4; - Hfusion.at<23>() = -HK5; + Hfusion.at<6>() = HK6; + Hfusion.at<22>() = HK7; + Hfusion.at<23>() = HK8; Vector24f Kfusion; // Kalman gain vector if (!update_wind_only) { // we have no other source of aiding, so use airspeed measurements to correct states - for (unsigned row = 0; row <= 3; row++) { - Kfusion(row) = HK16*(HK0*P(4,row) - HK0*P(row,22) + HK1*P(5,row) - HK1*P(row,23) + P(6,row)*vd); - } - - Kfusion(4) = HK12*HK16; - Kfusion(5) = HK16*HK9; - Kfusion(6) = HK16*HK7; - - for (unsigned row = 7; row <= 21; row++) { - Kfusion(row) = HK16*(HK0*P(4,row) - HK0*P(row,22) + HK1*P(5,row) - HK1*P(row,23) + P(6,row)*vd); + for (unsigned row = 0; row <= 21; row++) { + Kfusion(row) = HK9*(HK4*P(row,4) + HK5*P(row,5) + HK6*P(row,6) + HK7*P(row,22) + HK8*P(row,23)); } } - Kfusion(22) = HK15*HK16; - Kfusion(23) = HK14*HK16; - - // Calculate measurement innovation - _airspeed_innov = v_tas_pred - _airspeed_sample_delayed.true_airspeed; - - // Compute the ratio of innovation to gate size - _tas_test_ratio = sq(_airspeed_innov) / (sq(fmaxf(_params.tas_innov_gate, 1.0f)) * _airspeed_innov_var); - - // If the innovation consistency check fails then don't fuse the sample and indicate bad airspeed health - if (_tas_test_ratio > 1.0f) { - _innov_check_fail_status.flags.reject_airspeed = true; - return; - - } else { - _innov_check_fail_status.flags.reject_airspeed = false; - } + Kfusion(22) = HK9*(HK4*P(4,22) + HK5*P(5,22) + HK6*P(6,22) + HK7*P(22,22) + HK8*P(22,23)); + Kfusion(23) = HK9*(HK4*P(4,23) + HK5*P(5,23) + HK6*P(6,23) + HK7*P(22,23) + HK8*P(23,23)); - const bool is_fused = measurementUpdate(Kfusion, Hfusion, _airspeed_innov); + const bool is_fused = measurementUpdate(Kfusion, Hfusion, airspeed.innovation); + airspeed.fused = is_fused; _fault_status.flags.bad_airspeed = !is_fused; if (is_fused) { - _time_last_arsp_fuse = _time_last_imu; + airspeed.time_last_fuse = _time_last_imu; } } @@ -195,7 +212,7 @@ void Ekf::resetWindUsingAirspeed() resetWindCovarianceUsingAirspeed(); - _time_last_arsp_fuse = _time_last_imu; + _aid_src_airspeed.time_last_fuse = _time_last_imu; } void Ekf::resetWindToZero() diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index aa06c0d354..f5b2b6d8e6 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -1009,7 +1009,7 @@ void Ekf::controlAirDataFusion() // control activation and initialisation/reset of wind states required for airspeed fusion // If both airspeed and sideslip fusion have timed out and we are not using a drag observation model then we no longer have valid wind estimates - const bool airspeed_timed_out = isTimedOut(_time_last_arsp_fuse, (uint64_t)10e6); + const bool airspeed_timed_out = isTimedOut(_aid_src_airspeed.time_last_fuse, (uint64_t)10e6); const bool sideslip_timed_out = isTimedOut(_time_last_beta_fuse, (uint64_t)10e6); if (_using_synthetic_position || (airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & SensorFusionMask::USE_DRAG))) { @@ -1022,6 +1022,10 @@ void Ekf::controlAirDataFusion() } if (_tas_data_ready) { + updateAirspeed(_airspeed_sample_delayed, _aid_src_airspeed); + + _innov_check_fail_status.flags.reject_airspeed = _aid_src_airspeed.innovation_rejected; // TODO: remove this redundant flag + const bool continuing_conditions_passing = _control_status.flags.in_air && _control_status.flags.fixed_wing && !_using_synthetic_position; const bool is_airspeed_significant = _airspeed_sample_delayed.true_airspeed > _params.arsp_thr; const bool starting_conditions_passing = continuing_conditions_passing && is_airspeed_significant; @@ -1029,10 +1033,10 @@ void Ekf::controlAirDataFusion() if (_control_status.flags.fuse_aspd) { if (continuing_conditions_passing) { if (is_airspeed_significant) { - fuseAirspeed(); + fuseAirspeed(_aid_src_airspeed); } - const bool is_fusion_failing = isTimedOut(_time_last_arsp_fuse, (uint64_t)10e6); + const bool is_fusion_failing = isTimedOut(_aid_src_airspeed.time_last_fuse, (uint64_t)10e6); if (is_fusion_failing) { stopAirspeedFusion(); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 66399646a3..762369887a 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -123,9 +123,9 @@ public: void getDragInnovVar(float drag_innov_var[2]) const { _drag_innov_var.copyTo(drag_innov_var); } void getDragInnovRatio(float drag_innov_ratio[2]) const { _drag_test_ratio.copyTo(drag_innov_ratio); } - void getAirspeedInnov(float &airspeed_innov) const { airspeed_innov = _airspeed_innov; } - void getAirspeedInnovVar(float &airspeed_innov_var) const { airspeed_innov_var = _airspeed_innov_var; } - void getAirspeedInnovRatio(float &airspeed_innov_ratio) const { airspeed_innov_ratio = _tas_test_ratio; } + void getAirspeedInnov(float &airspeed_innov) const { airspeed_innov = _aid_src_airspeed.innovation; } + void getAirspeedInnovVar(float &airspeed_innov_var) const { airspeed_innov_var = _aid_src_airspeed.innovation_variance; } + void getAirspeedInnovRatio(float &airspeed_innov_ratio) const { airspeed_innov_ratio = _aid_src_airspeed.test_ratio; } void getBetaInnov(float &beta_innov) const { beta_innov = _beta_innov; } void getBetaInnovVar(float &beta_innov_var) const { beta_innov_var = _beta_innov_var; } @@ -412,7 +412,6 @@ private: uint64_t _time_last_ver_vel_fuse{0}; ///< time the last fusion of verticalvelocity measurements was performed (uSec) uint64_t _time_last_of_fuse{0}; ///< time the last fusion of optical flow measurements were performed (uSec) uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec) - uint64_t _time_last_arsp_fuse{0}; ///< time the last fusion of airspeed measurements were performed (uSec) uint64_t _time_last_beta_fuse{0}; ///< time the last fusion of synthetic sideslip measurements were performed (uSec) uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec) uint64_t _time_last_gps_yaw_fuse{0}; ///< time the last fusion of GPS yaw measurements were performed (uSec) @@ -475,9 +474,6 @@ private: Vector2f _drag_innov{}; ///< multirotor drag measurement innovation (m/sec**2) Vector2f _drag_innov_var{}; ///< multirotor drag measurement innovation variance ((m/sec**2)**2) - float _airspeed_innov{0.0f}; ///< airspeed measurement innovation (m/sec) - float _airspeed_innov_var{0.0f}; ///< airspeed measurement innovation variance ((m/sec)**2) - float _beta_innov{0.0f}; ///< synthetic sideslip measurement innovation (rad) float _beta_innov_var{0.0f}; ///< synthetic sideslip measurement innovation variance (rad**2) @@ -500,6 +496,7 @@ private: estimator_aid_source_1d_s _aid_src_baro_hgt{}; estimator_aid_source_1d_s _aid_src_rng_hgt{}; + estimator_aid_source_1d_s _aid_src_airspeed{}; estimator_aid_source_2d_s _aid_src_fake_pos{}; @@ -643,8 +640,8 @@ private: // apply sensible limits to the declination and length of the NE mag field states estimates void limitDeclination(); - // fuse airspeed measurement - void fuseAirspeed(); + void updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source_1d_s &airspeed) const; + void fuseAirspeed(estimator_aid_source_1d_s &airspeed); // fuse synthetic zero sideslip measurement void fuseSideslip(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index a8a7a08a2e..6e895adfe7 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -984,7 +984,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f } // return the airspeed fusion innovation test ratio - tas = sqrtf(_tas_test_ratio); + tas = sqrtf(_aid_src_airspeed.test_ratio); // return the terrain height innovation test ratio hagl = sqrtf(_hagl_test_ratio); @@ -1045,7 +1045,7 @@ void Ekf::update_deadreckoning_status() const bool optFlowAiding = _control_status.flags.opt_flow && isRecent(_time_last_of_fuse, _params.no_aid_timeout_max); const bool airDataAiding = _control_status.flags.wind && - isRecent(_time_last_arsp_fuse, _params.no_aid_timeout_max) && + isRecent(_aid_src_airspeed.time_last_fuse, _params.no_aid_timeout_max) && isRecent(_time_last_beta_fuse, _params.no_aid_timeout_max); _control_status.flags.wind_dead_reckoning = !velPosAiding && !optFlowAiding && airDataAiding; diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 6693c2662e..15e46cbbbb 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -335,7 +335,6 @@ protected: Vector2f _ev_pos_test_ratio{}; // EV position innovation consistency check ratios Vector2f _aux_vel_test_ratio{}; // Auxiliary horizontal velocity innovation consistency check ratio float _optflow_test_ratio{}; // Optical flow innovation consistency check ratio - float _tas_test_ratio{}; // tas innovation consistency check ratio float _hagl_test_ratio{}; // height above terrain measurement innovation consistency check ratio float _beta_test_ratio{}; // sideslip innovation consistency check ratio Vector2f _drag_test_ratio{}; // drag innovation consistency check ratio