dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: drag fusion add aid source status topic

This commit is contained in:
Daniel Agar 2023-10-05 13:15:30 -04:00
parent d61743412c
commit 534071573a
7 changed files with 70 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/EstimatorAidSource2d.msg
View File

@ -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_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_aux_vel estimator_aid_src_optical_flow estimator_aid_src_terrain_optical_flow
# TOPICS estimator_aid_src_drag

2
src/modules/ekf2/EKF/control.cpp
View File

@ -122,7 +122,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
#endif // CONFIG_EKF2_SIDESLIP #endif // CONFIG_EKF2_SIDESLIP
#if defined(CONFIG_EKF2_DRAG_FUSION) #if defined(CONFIG_EKF2_DRAG_FUSION)
controlDragFusion(); controlDragFusion(imu_delayed);
#endif // CONFIG_EKF2_DRAG_FUSION #endif // CONFIG_EKF2_DRAG_FUSION
controlHeightFusion(imu_delayed); controlHeightFusion(imu_delayed);

59
src/modules/ekf2/EKF/drag_fusion.cpp
View File

@ -42,12 +42,11 @@
#include <mathlib/mathlib.h> #include <mathlib/mathlib.h>
void Ekf::controlDragFusion() void Ekf::controlDragFusion(const imuSample &imu_delayed)
{ {
if ((_params.drag_ctrl > 0) && _drag_buffer && if ((_params.drag_ctrl > 0) && _drag_buffer) {
!_control_status.flags.fake_pos && _control_status.flags.in_air) {
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 // reset the wind states and covariances when starting drag accel fusion
_control_status.flags.wind = true; _control_status.flags.wind = true;
resetWindToZero(); resetWindToZero();
@ -55,7 +54,7 @@ void Ekf::controlDragFusion()
dragSample drag_sample; 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); fuseDrag(drag_sample);
} }
} }
@ -87,14 +86,14 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
const float rel_wind_speed = rel_wind_body.norm(); const float rel_wind_speed = rel_wind_body.norm();
const VectorState state_vector_prev = getStateAtFusionHorizonAsVector(); const VectorState state_vector_prev = getStateAtFusionHorizonAsVector();
Vector2f bcoef_inv; Vector2f bcoef_inv{0.f, 0.f};
if (using_bcoef_x) { if (using_bcoef_x) {
bcoef_inv(0) = 1.0f / _params.bcoef_x; bcoef_inv(0) = 1.f / _params.bcoef_x;
} }
if (using_bcoef_y) { 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) { if (using_bcoef_x && using_bcoef_y) {
@ -105,6 +104,11 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
bcoef_inv(1) = bcoef_inv(0); 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; VectorState Kfusion;
// perform sequential fusion of XY specific forces // 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 // 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 // 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. // 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) { 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) { if (!using_bcoef_x && !using_mcoef) {
continue; 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) { } 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) { if (!using_bcoef_y && !using_mcoef) {
continue; 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 // calculation is badly conditioned
return; 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 // Apply an innovation consistency check with a 5 Sigma threshold
_drag_innov(axis_index) = pred_acc - mea_acc; const float innov_gate = 5.f;
_drag_test_ratio(axis_index) = sq(_drag_innov(axis_index)) / (sq(5.0f) * _drag_innov_var(axis_index)); setEstimatorAidStatusTestRatio(_aid_src_drag, innov_gate);
// if the innovation consistency check fails then don't fuse the sample if (_control_status.flags.in_air && _control_status.flags.wind && !_control_status.flags.fake_pos
if (_drag_test_ratio(axis_index) <= 1.0f) { && PX4_ISFINITE(_aid_src_drag.innovation_variance[axis_index]) && PX4_ISFINITE(_aid_src_drag.innovation[axis_index])
measurementUpdate(Kfusion, _drag_innov_var(axis_index), _drag_innov(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;
}
} }

25
src/modules/ekf2/EKF/ekf.h
View File

@ -282,9 +282,23 @@ public:
#endif // CONFIG_EKF2_MAGNETOMETER #endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_DRAG_FUSION) #if defined(CONFIG_EKF2_DRAG_FUSION)
void getDragInnov(float drag_innov[2]) const { _drag_innov.copyTo(drag_innov); } const auto &aid_src_drag() const { return _aid_src_drag; }
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 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 #endif // CONFIG_EKF2_DRAG_FUSION
#if defined(CONFIG_EKF2_AIRSPEED) #if defined(CONFIG_EKF2_AIRSPEED)
@ -622,8 +636,7 @@ private:
SquareMatrixState P{}; ///< state covariance matrix SquareMatrixState P{}; ///< state covariance matrix
#if defined(CONFIG_EKF2_DRAG_FUSION) #if defined(CONFIG_EKF2_DRAG_FUSION)
Vector2f _drag_innov{}; ///< multirotor drag measurement innovation (m/sec**2) estimator_aid_source2d_s _aid_src_drag{};
Vector2f _drag_innov_var{}; ///< multirotor drag measurement innovation variance ((m/sec**2)**2)
#endif // CONFIG_EKF2_DRAG_FUSION #endif // CONFIG_EKF2_DRAG_FUSION
#if defined(CONFIG_EKF2_TERRAIN) #if defined(CONFIG_EKF2_TERRAIN)
@ -867,7 +880,7 @@ private:
#if defined(CONFIG_EKF2_DRAG_FUSION) #if defined(CONFIG_EKF2_DRAG_FUSION)
// control fusion of multi-rotor drag specific force observations // 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 // fuse body frame drag specific forces for multi-rotor wind estimation
void fuseDrag(const dragSample &drag_sample); void fuseDrag(const dragSample &drag_sample);

1
src/modules/ekf2/EKF/estimator_interface.h
View File

@ -397,7 +397,6 @@ protected:
#if defined(CONFIG_EKF2_DRAG_FUSION) #if defined(CONFIG_EKF2_DRAG_FUSION)
RingBuffer<dragSample> *_drag_buffer{nullptr}; RingBuffer<dragSample> *_drag_buffer{nullptr};
dragSample _drag_down_sampled{}; // down sampled drag specific force data (filter prediction rate -> observation rate) 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 #endif // CONFIG_EKF2_DRAG_FUSION
innovation_fault_status_u _innov_check_fail_status{}; innovation_fault_status_u _innov_check_fail_status{};

5
src/modules/ekf2/EKF2.cpp
View File

@ -1002,6 +1002,11 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
PublishAidSourceStatus(_ekf.aid_src_baro_hgt(), _status_baro_hgt_pub_last, _estimator_aid_src_baro_hgt_pub); PublishAidSourceStatus(_ekf.aid_src_baro_hgt(), _status_baro_hgt_pub_last, _estimator_aid_src_baro_hgt_pub);
#endif // CONFIG_EKF2_BAROMETER #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) #if defined(CONFIG_EKF2_RANGE_FINDER)
// RNG height // RNG height
PublishAidSourceStatus(_ekf.aid_src_rng_hgt(), _status_rng_hgt_pub_last, _estimator_aid_src_rng_hgt_pub); PublishAidSourceStatus(_ekf.aid_src_rng_hgt(), _status_rng_hgt_pub_last, _estimator_aid_src_rng_hgt_pub);

5
src/modules/ekf2/EKF2.hpp
View File

@ -404,6 +404,11 @@ private:
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_baro_hgt_pub {ORB_ID(estimator_aid_src_baro_hgt)}; uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_baro_hgt_pub {ORB_ID(estimator_aid_src_baro_hgt)};
#endif // CONFIG_EKF2_BAROMETER #endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_DRAG_FUSION)
uORB::PublicationMulti<estimator_aid_source2d_s> _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{}; float _last_gnss_hgt_bias_published{};
#if defined(CONFIG_EKF2_AIRSPEED) #if defined(CONFIG_EKF2_AIRSPEED)