fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 02:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: add kconfig option to enable/disable drag fusion

Browse Source
This commit is contained in:
Daniel Agar
2023-03-20 10:12:17 -04:00
parent 9a56b0a70d
commit fe0e3acf09
11 changed files with 115 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/CMakeLists.txt
+10 -2
View File
@@ -31,14 +31,14 @@
#
############################################################################
add_library(ecl_EKF
set(EKF_SRCS)
list(APPEND EKF_SRCS
airspeed_fusion.cpp
auxvel_fusion.cpp
baro_height_control.cpp
bias_estimator.cpp
control.cpp
covariance.cpp
drag_fusion.cpp
ekf.cpp
ekf_helper.cpp
EKFGSF_yaw.cpp
@@ -72,6 +72,14 @@ add_library(ecl_EKF
zero_velocity_update.cpp
)
if(CONFIG_EKF2_DRAG_FUSION)
list(APPEND EKF_SRCS drag_fusion.cpp)
endif()
add_library(ecl_EKF
${EKF_SRCS}
)
add_dependencies(ecl_EKF prebuild_targets)
target_link_libraries(ecl_EKF PRIVATE geo world_magnetic_model)
target_compile_options(ecl_EKF PRIVATE -fno-associative-math)
src/modules/ekf2/EKF/common.h
+4
View File
@@ -252,10 +252,12 @@ struct extVisionSample {
int8_t quality{}; ///< quality indicator between 0 and 100
};
#if defined(CONFIG_EKF2_DRAG_FUSION)
struct dragSample {
uint64_t time_us{}; ///< timestamp of the measurement (uSec)
Vector2f accelXY{}; ///< measured specific force along the X and Y body axes (m/sec**2)
};
#endif // CONFIG_EKF2_DRAG_FUSION
struct auxVelSample {
uint64_t time_us{}; ///< timestamp of the measurement (uSec)
@@ -446,11 +448,13 @@ struct parameters {
// upper limit on airspeed used for correction (m/s**2)
float max_correction_airspeed {20.0f};
#if defined(CONFIG_EKF2_DRAG_FUSION)
// multi-rotor drag specific force fusion
float drag_noise{2.5f}; ///< observation noise variance for drag specific force measurements (m/sec**2)**2
float bcoef_x{100.0f}; ///< bluff body drag ballistic coefficient for the X-axis (kg/m**2)
float bcoef_y{100.0f}; ///< bluff body drag ballistic coefficient for the Y-axis (kg/m**2)
float mcoef{0.1f}; ///< rotor momentum drag coefficient for the X and Y axes (1/s)
#endif // CONFIG_EKF2_DRAG_FUSION
// control of accel error detection and mitigation (IMU clipping)
const float vert_innov_test_lim{3.0f}; ///< Number of standard deviations of vertical vel/pos innovations allowed before triggering a vertical acceleration failure
src/modules/ekf2/EKF/control.cpp
+4
View File
@@ -108,7 +108,11 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
controlGpsFusion(imu_delayed);
controlAirDataFusion(imu_delayed);
controlBetaFusion(imu_delayed);
#if defined(CONFIG_EKF2_DRAG_FUSION)
controlDragFusion();
#endif // CONFIG_EKF2_DRAG_FUSION
controlHeightFusion(imu_delayed);
controlGravityFusion(imu_delayed);
src/modules/ekf2/EKF/ekf.h
+9 -3
View File
@@ -171,9 +171,11 @@ public:
void getMagInnovVar(float mag_innov_var[3]) const { memcpy(mag_innov_var, _aid_src_mag.innovation_variance, sizeof(_aid_src_mag.innovation_variance)); }
void getMagInnovRatio(float &mag_innov_ratio) const { mag_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max(); }
#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); }
#endif // CONFIG_EKF2_DRAG_FUSION
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; }
@@ -537,8 +539,10 @@ private:
Vector3f _delta_angle_bias_var_accum{}; ///< kahan summation algorithm accumulator for delta angle bias variance
Vector3f _delta_vel_bias_var_accum{}; ///< kahan summation algorithm accumulator for delta velocity bias variance
#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)
#endif // CONFIG_EKF2_DRAG_FUSION
float _hagl_innov{0.0f}; ///< innovation of the last height above terrain measurement (m)
float _hagl_innov_var{0.0f}; ///< innovation variance for the last height above terrain measurement (m**2)
@@ -685,8 +689,13 @@ private:
void updateSideslip(estimator_aid_source1d_s &_aid_src_sideslip) const;
void fuseSideslip(estimator_aid_source1d_s &_aid_src_sideslip);
#if defined(CONFIG_EKF2_DRAG_FUSION)
// control fusion of multi-rotor drag specific force observations
void controlDragFusion();
// fuse body frame drag specific forces for multi-rotor wind estimation
void fuseDrag(const dragSample &drag_sample);
#endif // CONFIG_EKF2_DRAG_FUSION
// fuse single velocity and position measurement
bool fuseVelPosHeight(const float innov, const float innov_var, const int obs_index);
@@ -914,9 +923,6 @@ private:
// control fusion of synthetic sideslip observations
void controlBetaFusion(const imuSample &imu_delayed);
// control fusion of multi-rotor drag specific force observations
void controlDragFusion();
// control fusion of fake position observations to constrain drift
void controlFakePosFusion();
src/modules/ekf2/EKF/estimator_interface.cpp
+8
View File
@@ -53,7 +53,9 @@ EstimatorInterface::~EstimatorInterface()
delete _airspeed_buffer;
delete _flow_buffer;
delete _ext_vision_buffer;
#if defined(CONFIG_EKF2_DRAG_FUSION)
delete _drag_buffer;
#endif // CONFIG_EKF2_DRAG_FUSION
delete _auxvel_buffer;
}
@@ -85,7 +87,9 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
_min_obs_interval_us = (imu_sample.time_us - _time_delayed_us) / (_obs_buffer_length - 1);
}
#if defined(CONFIG_EKF2_DRAG_FUSION)
setDragData(imu_sample);
#endif // CONFIG_EKF2_DRAG_FUSION
}
void EstimatorInterface::setMagData(const magSample &mag_sample)
@@ -452,6 +456,7 @@ void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags)
}
}
#if defined(CONFIG_EKF2_DRAG_FUSION)
void EstimatorInterface::setDragData(const imuSample &imu)
{
// down-sample the drag specific force data by accumulating and calculating the mean when
@@ -502,6 +507,7 @@ void EstimatorInterface::setDragData(const imuSample &imu)
}
}
}
#endif // CONFIG_EKF2_DRAG_FUSION
bool EstimatorInterface::initialise_interface(uint64_t timestamp)
{
@@ -682,9 +688,11 @@ void EstimatorInterface::print_status()
printf("vision buffer: %d/%d (%d Bytes)\n", _ext_vision_buffer->entries(), _ext_vision_buffer->get_length(), _ext_vision_buffer->get_total_size());
}
#if defined(CONFIG_EKF2_DRAG_FUSION)
if (_drag_buffer) {
printf("drag buffer: %d/%d (%d Bytes)\n", _drag_buffer->entries(), _drag_buffer->get_length(), _drag_buffer->get_total_size());
}
#endif // CONFIG_EKF2_DRAG_FUSION
_output_predictor.print_status();
}
src/modules/ekf2/EKF/estimator_interface.h
+13 -8
View File
@@ -294,8 +294,6 @@ protected:
airspeedSample _airspeed_sample_delayed{};
flowSample _flow_sample_delayed{};
extVisionSample _ev_sample_prev{};
dragSample _drag_down_sampled{}; // down sampled drag specific force data (filter prediction rate -> observation rate)
RangeFinderConsistencyCheck _rng_consistency_check;
float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // air density (kg/m**3)
@@ -320,7 +318,13 @@ protected:
AlphaFilter<float> _gnss_yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state
float _hagl_test_ratio{}; // height above terrain measurement innovation consistency check ratio
#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{};
bool _horizontal_deadreckon_time_exceeded{true};
@@ -345,7 +349,6 @@ protected:
RingBuffer<airspeedSample> *_airspeed_buffer{nullptr};
RingBuffer<flowSample> *_flow_buffer{nullptr};
RingBuffer<extVisionSample> *_ext_vision_buffer{nullptr};
RingBuffer<dragSample> *_drag_buffer{nullptr};
RingBuffer<auxVelSample> *_auxvel_buffer{nullptr};
RingBuffer<systemFlagUpdate> *_system_flag_buffer{nullptr};
@@ -380,16 +383,18 @@ protected:
private:
inline void setDragData(const imuSample &imu);
#if defined(CONFIG_EKF2_DRAG_FUSION)
void setDragData(const imuSample &imu);
// Used by the multi-rotor specific drag force fusion
uint8_t _drag_sample_count{0}; // number of drag specific force samples assumulated at the filter prediction rate
float _drag_sample_time_dt{0.0f}; // time integral across all samples used to form _drag_down_sampled (sec)
#endif // CONFIG_EKF2_DRAG_FUSION
void printBufferAllocationFailed(const char *buffer_name);
ImuDownSampler _imu_down_sampler{_params.filter_update_interval_us};
unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec)
// Used by the multi-rotor specific drag force fusion
uint8_t _drag_sample_count{0}; // number of drag specific force samples assumulated at the filter prediction rate
float _drag_sample_time_dt{0.0f}; // time integral across all samples used to form _drag_down_sampled (sec)
};
#endif // !EKF_ESTIMATOR_INTERFACE_H