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

ekf2: use estimator_aid_source_1d message for airspeed fusion

Browse Source 
split the fusion process into:
1. updateAirspeed: computes innov, innov_var, obs_var, ...
2. fuseAirspeed: uses data computed in 1. to generate K, H and fuse the
   observation
This commit is contained in:
bresch
2022-07-07 09:42:54 -04:00
committed by Daniel Agar
parent 503aa87957
commit 2fd87c47e8
6 changed files with 88 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.h
+6 -9
View File
@@ -123,9 +123,9 @@ public:
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 getAirspeedInnov(float &airspeed_innov) const { airspeed_innov = _airspeed_innov; }
void getAirspeedInnovVar(float &airspeed_innov_var) const { airspeed_innov_var = _airspeed_innov_var; }
void getAirspeedInnovRatio(float &airspeed_innov_ratio) const { airspeed_innov_ratio = _tas_test_ratio; }
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; }
void getAirspeedInnovRatio(float &airspeed_innov_ratio) const { airspeed_innov_ratio = _aid_src_airspeed.test_ratio; }
void getBetaInnov(float &beta_innov) const { beta_innov = _beta_innov; }
void getBetaInnovVar(float &beta_innov_var) const { beta_innov_var = _beta_innov_var; }
@@ -412,7 +412,6 @@ private:
uint64_t _time_last_ver_vel_fuse{0}; ///< time the last fusion of verticalvelocity measurements was performed (uSec)
uint64_t _time_last_of_fuse{0}; ///< time the last fusion of optical flow measurements were performed (uSec)
uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec)
uint64_t _time_last_arsp_fuse{0}; ///< time the last fusion of airspeed measurements were performed (uSec)
uint64_t _time_last_beta_fuse{0}; ///< time the last fusion of synthetic sideslip measurements were performed (uSec)
uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec)
uint64_t _time_last_gps_yaw_fuse{0}; ///< time the last fusion of GPS yaw measurements were performed (uSec)
@@ -475,9 +474,6 @@ private:
Vector2f _drag_innov{}; ///< multirotor drag measurement innovation (m/sec**2)
Vector2f _drag_innov_var{}; ///< multirotor drag measurement innovation variance ((m/sec**2)**2)
float _airspeed_innov{0.0f}; ///< airspeed measurement innovation (m/sec)
float _airspeed_innov_var{0.0f}; ///< airspeed measurement innovation variance ((m/sec)**2)
float _beta_innov{0.0f}; ///< synthetic sideslip measurement innovation (rad)
float _beta_innov_var{0.0f}; ///< synthetic sideslip measurement innovation variance (rad**2)
@@ -500,6 +496,7 @@ private:
estimator_aid_source_1d_s _aid_src_baro_hgt{};
estimator_aid_source_1d_s _aid_src_rng_hgt{};
estimator_aid_source_1d_s _aid_src_airspeed{};
estimator_aid_source_2d_s _aid_src_fake_pos{};
@@ -643,8 +640,8 @@ private:
// apply sensible limits to the declination and length of the NE mag field states estimates
void limitDeclination();
// fuse airspeed measurement
void fuseAirspeed();
void updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source_1d_s &airspeed) const;
void fuseAirspeed(estimator_aid_source_1d_s &airspeed);
// fuse synthetic zero sideslip measurement
void fuseSideslip();