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

ekf2: perform GNSS checks at delayed-time horizon

Browse Source 
- never fuse a measurement that is not passing the checks
- cleanup and simplify GNSS vel/pos control logic
This commit is contained in:
bresch
2023-12-18 18:11:20 +01:00
committed by Mathieu Bresciani
parent b0566cd8aa
commit a653073d4f
15 changed files with 371 additions and 419 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/estimator_interface.h
+5 -7
View File
@@ -84,15 +84,14 @@ public:
void setIMUData(const imuSample &imu_sample);
#if defined(CONFIG_EKF2_GNSS)
// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
virtual bool collect_gps(const gpsMessage &gps) = 0;
void setGpsData(const gpsMessage &gps);
void setGpsData(const gnssSample &gnss_sample);
const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; }
const gnssSample &get_gps_sample_delayed() const { return _gps_sample_delayed; }
float gps_horizontal_position_drift_rate_m_s() const { return _gps_horizontal_position_drift_rate_m_s; }
float gps_vertical_position_drift_rate_m_s() const { return _gps_vertical_position_drift_rate_m_s; }
float gps_filtered_horizontal_velocity_m_s() const { return _gps_filtered_horizontal_velocity_m_s; }
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_MAGNETOMETER)
@@ -390,10 +389,10 @@ protected:
float _gpos_origin_epv{0.0f}; // vertical position uncertainty of the global origin
#if defined(CONFIG_EKF2_GNSS)
RingBuffer<gpsSample> *_gps_buffer{nullptr};
RingBuffer<gnssSample> *_gps_buffer{nullptr};
uint64_t _time_last_gps_buffer_push{0};
gpsSample _gps_sample_delayed{};
gnssSample _gps_sample_delayed{};
float _gps_horizontal_position_drift_rate_m_s{NAN}; // Horizontal position drift rate (m/s)
float _gps_vertical_position_drift_rate_m_s{NAN}; // Vertical position drift rate (m/s)
@@ -403,7 +402,6 @@ protected:
float _gps_alt_prev{0.0f}; // height from the previous GPS message (m)
# if defined(CONFIG_EKF2_GNSS_YAW)
float _gps_yaw_offset{0.0f}; // Yaw offset angle for dual GPS antennas used for yaw estimation (radians).
// innovation consistency check monitoring ratios
AlphaFilter<float> _gnss_yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state
uint64_t _time_last_gps_yaw_buffer_push{0};