From 534071573afb99891cd115fcc66daa6b90e83b87 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 5 Oct 2023 13:15:30 -0400 Subject: [PATCH] ekf2: drag fusion add aid source status topic --- msg/EstimatorAidSource2d.msg | 1 + src/modules/ekf2/EKF/control.cpp | 2 +- src/modules/ekf2/EKF/drag_fusion.cpp | 59 ++++++++++++++-------- src/modules/ekf2/EKF/ekf.h | 25 ++++++--- src/modules/ekf2/EKF/estimator_interface.h | 1 - src/modules/ekf2/EKF2.cpp | 5 ++ src/modules/ekf2/EKF2.hpp | 5 ++ 7 files changed, 70 insertions(+), 28 deletions(-) diff --git a/msg/EstimatorAidSource2d.msg b/msg/EstimatorAidSource2d.msg index 2d33fc361d..67074de143 100644 --- a/msg/EstimatorAidSource2d.msg +++ b/msg/EstimatorAidSource2d.msg @@ -19,3 +19,4 @@ bool fused # true if the sample was successfully fused # TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos # TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow estimator_aid_src_terrain_optical_flow +# TOPICS estimator_aid_src_drag diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 332aaf37b8..a1ad5f40f3 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -122,7 +122,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) #endif // CONFIG_EKF2_SIDESLIP #if defined(CONFIG_EKF2_DRAG_FUSION) - controlDragFusion(); + controlDragFusion(imu_delayed); #endif // CONFIG_EKF2_DRAG_FUSION controlHeightFusion(imu_delayed); diff --git a/src/modules/ekf2/EKF/drag_fusion.cpp b/src/modules/ekf2/EKF/drag_fusion.cpp index a11a599be7..7a715caeef 100644 --- a/src/modules/ekf2/EKF/drag_fusion.cpp +++ b/src/modules/ekf2/EKF/drag_fusion.cpp @@ -42,12 +42,11 @@ #include -void Ekf::controlDragFusion() +void Ekf::controlDragFusion(const imuSample &imu_delayed) { - if ((_params.drag_ctrl > 0) && _drag_buffer && - !_control_status.flags.fake_pos && _control_status.flags.in_air) { + if ((_params.drag_ctrl > 0) && _drag_buffer) { - if (!_control_status.flags.wind) { + if (!_control_status.flags.wind && !_control_status.flags.fake_pos && _control_status.flags.in_air) { // reset the wind states and covariances when starting drag accel fusion _control_status.flags.wind = true; resetWindToZero(); @@ -55,7 +54,7 @@ void Ekf::controlDragFusion() dragSample drag_sample; - if (_drag_buffer->pop_first_older_than(_time_delayed_us, &drag_sample)) { + if (_drag_buffer->pop_first_older_than(imu_delayed.time_us, &drag_sample)) { fuseDrag(drag_sample); } } @@ -87,14 +86,14 @@ void Ekf::fuseDrag(const dragSample &drag_sample) const float rel_wind_speed = rel_wind_body.norm(); const VectorState state_vector_prev = getStateAtFusionHorizonAsVector(); - Vector2f bcoef_inv; + Vector2f bcoef_inv{0.f, 0.f}; if (using_bcoef_x) { - bcoef_inv(0) = 1.0f / _params.bcoef_x; + bcoef_inv(0) = 1.f / _params.bcoef_x; } if (using_bcoef_y) { - bcoef_inv(1) = 1.0f / _params.bcoef_y; + bcoef_inv(1) = 1.f / _params.bcoef_y; } if (using_bcoef_x && using_bcoef_y) { @@ -105,6 +104,11 @@ void Ekf::fuseDrag(const dragSample &drag_sample) bcoef_inv(1) = bcoef_inv(0); } + _aid_src_drag.timestamp_sample = drag_sample.time_us; + _aid_src_drag.fused = false; + + bool fused[] {false, false}; + VectorState Kfusion; // perform sequential fusion of XY specific forces @@ -115,36 +119,51 @@ void Ekf::fuseDrag(const dragSample &drag_sample) // Drag is modelled as an arbitrary combination of bluff body drag that proportional to // equivalent airspeed squared, and rotor momentum drag that is proportional to true airspeed // parallel to the rotor disc and mass flow through the rotor disc. + const float pred_acc = -0.5f * bcoef_inv(axis_index) * rho * rel_wind_body(axis_index) * rel_wind_speed - rel_wind_body(axis_index) * mcoef_corrrected; + + _aid_src_drag.observation[axis_index] = mea_acc; + _aid_src_drag.observation_variance[axis_index] = R_ACC; + _aid_src_drag.innovation[axis_index] = pred_acc - mea_acc; + _aid_src_drag.innovation_variance[axis_index] = NAN; // reset if (axis_index == 0) { + sym::ComputeDragXInnovVarAndK(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON, + &_aid_src_drag.innovation_variance[axis_index], &Kfusion); + if (!using_bcoef_x && !using_mcoef) { continue; } - sym::ComputeDragXInnovVarAndK(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON, &_drag_innov_var(axis_index), &Kfusion); - } else if (axis_index == 1) { + sym::ComputeDragYInnovVarAndK(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON, + &_aid_src_drag.innovation_variance[axis_index], &Kfusion); + if (!using_bcoef_y && !using_mcoef) { continue; } - - sym::ComputeDragYInnovVarAndK(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON, &_drag_innov_var(axis_index), &Kfusion); } - if (_drag_innov_var(axis_index) < R_ACC) { + if (_aid_src_drag.innovation_variance[axis_index] < R_ACC) { // calculation is badly conditioned return; } - const float pred_acc = -0.5f * bcoef_inv(axis_index) * rho * rel_wind_body(axis_index) * rel_wind_speed - rel_wind_body(axis_index) * mcoef_corrrected; - // Apply an innovation consistency check with a 5 Sigma threshold - _drag_innov(axis_index) = pred_acc - mea_acc; - _drag_test_ratio(axis_index) = sq(_drag_innov(axis_index)) / (sq(5.0f) * _drag_innov_var(axis_index)); + const float innov_gate = 5.f; + setEstimatorAidStatusTestRatio(_aid_src_drag, innov_gate); - // if the innovation consistency check fails then don't fuse the sample - if (_drag_test_ratio(axis_index) <= 1.0f) { - measurementUpdate(Kfusion, _drag_innov_var(axis_index), _drag_innov(axis_index)); + if (_control_status.flags.in_air && _control_status.flags.wind && !_control_status.flags.fake_pos + && PX4_ISFINITE(_aid_src_drag.innovation_variance[axis_index]) && PX4_ISFINITE(_aid_src_drag.innovation[axis_index]) + && (_aid_src_drag.test_ratio[axis_index] < 1.f) + ) { + if (measurementUpdate(Kfusion, _aid_src_drag.innovation_variance[axis_index], _aid_src_drag.innovation[axis_index])) { + fused[axis_index] = true; + } } } + + if (fused[0] && fused[1]) { + _aid_src_drag.fused = true; + _aid_src_drag.time_last_fuse = _time_delayed_us; + } } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 1896a8ea8a..966c87f9e5 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -282,9 +282,23 @@ public: #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_DRAG_FUSION) - void getDragInnov(float drag_innov[2]) const { _drag_innov.copyTo(drag_innov); } - 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); } + const auto &aid_src_drag() const { return _aid_src_drag; } + + void getDragInnov(float drag_innov[2]) const + { + drag_innov[0] = _aid_src_drag.innovation[0]; + drag_innov[1] = _aid_src_drag.innovation[1]; + } + void getDragInnovVar(float drag_innov_var[2]) const + { + drag_innov_var[0] = _aid_src_drag.innovation_variance[0]; + drag_innov_var[1] = _aid_src_drag.innovation_variance[1]; + } + void getDragInnovRatio(float drag_innov_ratio[2]) const + { + drag_innov_ratio[0] = _aid_src_drag.test_ratio[0]; + drag_innov_ratio[1] = _aid_src_drag.test_ratio[1]; + } #endif // CONFIG_EKF2_DRAG_FUSION #if defined(CONFIG_EKF2_AIRSPEED) @@ -622,8 +636,7 @@ private: SquareMatrixState P{}; ///< state covariance matrix #if defined(CONFIG_EKF2_DRAG_FUSION) - Vector2f _drag_innov{}; ///< multirotor drag measurement innovation (m/sec**2) - Vector2f _drag_innov_var{}; ///< multirotor drag measurement innovation variance ((m/sec**2)**2) + estimator_aid_source2d_s _aid_src_drag{}; #endif // CONFIG_EKF2_DRAG_FUSION #if defined(CONFIG_EKF2_TERRAIN) @@ -867,7 +880,7 @@ private: #if defined(CONFIG_EKF2_DRAG_FUSION) // control fusion of multi-rotor drag specific force observations - void controlDragFusion(); + void controlDragFusion(const imuSample &imu_delayed); // fuse body frame drag specific forces for multi-rotor wind estimation void fuseDrag(const dragSample &drag_sample); diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 53881bb95e..f09745ec98 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -397,7 +397,6 @@ protected: #if defined(CONFIG_EKF2_DRAG_FUSION) RingBuffer *_drag_buffer{nullptr}; dragSample _drag_down_sampled{}; // down sampled drag specific force data (filter prediction rate -> observation rate) - Vector2f _drag_test_ratio{}; // drag innovation consistency check ratio #endif // CONFIG_EKF2_DRAG_FUSION innovation_fault_status_u _innov_check_fail_status{}; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index e2aa9507b2..fe58b9467c 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1002,6 +1002,11 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) PublishAidSourceStatus(_ekf.aid_src_baro_hgt(), _status_baro_hgt_pub_last, _estimator_aid_src_baro_hgt_pub); #endif // CONFIG_EKF2_BAROMETER +#if defined(CONFIG_EKF2_DRAG_FUSION) + // drag + PublishAidSourceStatus(_ekf.aid_src_drag(), _status_drag_pub_last, _estimator_aid_src_drag_pub); +#endif // CONFIG_EKF2_DRAG_FUSION + #if defined(CONFIG_EKF2_RANGE_FINDER) // RNG height PublishAidSourceStatus(_ekf.aid_src_rng_hgt(), _status_rng_hgt_pub_last, _estimator_aid_src_rng_hgt_pub); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 8eea2e9dec..9caf425e0d 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -404,6 +404,11 @@ private: uORB::PublicationMulti _estimator_aid_src_baro_hgt_pub {ORB_ID(estimator_aid_src_baro_hgt)}; #endif // CONFIG_EKF2_BAROMETER +#if defined(CONFIG_EKF2_DRAG_FUSION) + uORB::PublicationMulti _estimator_aid_src_drag_pub {ORB_ID(estimator_aid_src_drag)}; + hrt_abstime _status_drag_pub_last{0}; +#endif // CONFIG_EKF2_DRAG_FUSION + float _last_gnss_hgt_bias_published{}; #if defined(CONFIG_EKF2_AIRSPEED)