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

AlphaFilter: set time constant instead of alpha

Browse Source 
Then the update function can set the dt at every iteration if needed
This commit is contained in:
bresch
2024-12-18 14:11:08 +01:00
committed by Matthias Grob
parent 37401d6fd1
commit fa5a781e20
4 changed files with 16 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.h
+9 -7
View File
@@ -518,7 +518,7 @@ private:
Vector3f _ref_body_rate{};
Vector2f _flow_vel_body{}; ///< velocity from corrected flow measurement (body frame)(m/s)
AlphaFilter<Vector2f> _flow_vel_body_lpf{0.1f}; ///< filtered velocity from corrected flow measurement (body frame)(m/s)
AlphaFilter<Vector2f> _flow_vel_body_lpf{_dt_ekf_avg, _kSensorLpfTimeConstant}; ///< filtered velocity from corrected flow measurement (body frame)(m/s)
uint32_t _flow_counter{0}; ///< number of flow samples read for initialization
Vector2f _flow_rate_compensated{}; ///< measured angular rate of the image about the X and Y body axes after removal of body rotation (rad/s), RH rotation is positive
@@ -583,14 +583,15 @@ private:
// Variables used by the initial filter alignment
bool _is_first_imu_sample{true};
AlphaFilter<Vector3f> _accel_lpf{0.1f}; ///< filtered accelerometer measurement used to align tilt (m/s/s)
AlphaFilter<Vector3f> _gyro_lpf{0.1f}; ///< filtered gyro measurement used for alignment excessive movement check (rad/sec)
static constexpr float _kSensorLpfTimeConstant = 0.09f;
AlphaFilter<Vector3f> _accel_lpf{_dt_ekf_avg, _kSensorLpfTimeConstant}; ///< filtered accelerometer measurement used to align tilt (m/s/s)
AlphaFilter<Vector3f> _gyro_lpf{_dt_ekf_avg, _kSensorLpfTimeConstant}; ///< filtered gyro measurement used for alignment excessive movement check (rad/sec)
#if defined(CONFIG_EKF2_BAROMETER)
estimator_aid_source1d_s _aid_src_baro_hgt {};
// Variables used to perform in flight resets and switch between height sources
AlphaFilter<float> _baro_lpf{0.1f}; ///< filtered barometric height measurement (m)
AlphaFilter<float> _baro_lpf{_dt_ekf_avg, _kSensorLpfTimeConstant}; ///< filtered barometric height measurement (m)
uint32_t _baro_counter{0}; ///< number of baro samples read during initialisation
HeightBiasEstimator _baro_b_est{HeightSensor::BARO, _height_sensor_ref};
@@ -600,12 +601,12 @@ private:
#if defined(CONFIG_EKF2_MAGNETOMETER)
// used by magnetometer fusion mode selection
AlphaFilter<float> _mag_heading_innov_lpf{0.1f};
AlphaFilter<float> _mag_heading_innov_lpf{_dt_ekf_avg, _kSensorLpfTimeConstant};
uint32_t _min_mag_health_time_us{1'000'000}; ///< magnetometer is marked as healthy only after this amount of time
estimator_aid_source3d_s _aid_src_mag{};
AlphaFilter<Vector3f> _mag_lpf{0.1f}; ///< filtered magnetometer measurement for instant reset (Gauss)
AlphaFilter<Vector3f> _mag_lpf{_dt_ekf_avg, _kSensorLpfTimeConstant}; ///< filtered magnetometer measurement for instant reset (Gauss)
uint32_t _mag_counter{0}; ///< number of magnetometer samples read during initialisation
// Variables used to control activation of post takeoff functionality
@@ -1011,7 +1012,8 @@ private:
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
HeightBiasEstimator _ev_hgt_b_est {HeightSensor::EV, _height_sensor_ref};
PositionBiasEstimator _ev_pos_b_est{PositionSensor::EV, _position_sensor_ref};
AlphaFilter<Quatf> _ev_q_error_filt{0.001f};
static constexpr float _kQuatErrorLpfTimeConstant = 10.f;
AlphaFilter<Quatf> _ev_q_error_filt{_dt_ekf_avg, _kQuatErrorLpfTimeConstant};
bool _ev_q_error_initialized{false};
#endif // CONFIG_EKF2_EXTERNAL_VISION