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 028733e1c7
commit 5f87f3a046
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_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
#if defined(CONFIG_EKF2_DRAG_FUSION)
controlDragFusion();
controlDragFusion(imu_delayed);
#endif // CONFIG_EKF2_DRAG_FUSION
controlHeightFusion(imu_delayed);

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

@ -42,12 +42,11 @@
#include <mathlib/mathlib.h>
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 = _state.vector();
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;
}
}

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

@ -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)
@ -625,8 +639,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)
@ -868,7 +881,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);

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

@ -397,7 +397,6 @@ protected:
#if defined(CONFIG_EKF2_DRAG_FUSION)
RingBuffer<dragSample> *_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{};

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

@ -1012,6 +1012,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);
#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);

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

@ -408,6 +408,11 @@ private:
uORB::PublicationMulti<estimator_aid_source1d_s> _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_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{};
#if defined(CONFIG_EKF2_AIRSPEED)