fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 12:53:33 +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/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();