fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-20 07:29:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: use alpha filter class

Browse Source
This commit is contained in:
bresch 2024-12-16 12:01:36 +01:00 committed by Matthias Grob
parent fa5a781e20
commit 480c232bfd
4 changed files with 13 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp
View File

@ -482,7 +482,7 @@ void Ekf::checkMagHeadingConsistency(const magSample &mag_sample)
// Check if there has been enough change in horizontal velocity to make yaw observable
const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos;
if (using_ne_aiding && (_accel_lpf_NE.norm() > _params.mag_acc_gate)) {
if (using_ne_aiding && (_accel_horiz_lpf.getState().longerThan(_params.mag_acc_gate))) {
// yaw angle must be observable to consider consistency
_control_status.flags.mag_heading_consistent = true;
}

5
src/modules/ekf2/EKF/covariance.cpp
View File

@ -202,8 +202,9 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
#if defined(CONFIG_EKF2_WIND)
// wind vel: add process noise
float wind_vel_nsd_scaled = _params.wind_vel_nsd * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf));
float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt;
const float height_rate = _height_rate_lpf.update(_state.vel(2), imu_delayed.delta_vel_dt);
const float wind_vel_nsd_scaled = _params.wind_vel_nsd * (1.f + _params.wind_vel_nsd_scaler * fabsf(height_rate));
const float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt;
for (unsigned index = 0; index < State::wind_vel.dof; index++) {
const unsigned i = State::wind_vel.idx + index;

11
src/modules/ekf2/EKF/ekf.cpp
View File

@ -272,15 +272,8 @@ void Ekf::predictState(const imuSample &imu_delayed)
// constrain states
_state.vel = matrix::constrain(_state.vel, -_params.velocity_limit, _params.velocity_limit);
// calculate a filtered horizontal acceleration with a 1 sec time constant
// this are used for manoeuvre detection elsewhere
const float alpha = 1.0f - imu_delayed.delta_vel_dt;
_accel_lpf_NE = _accel_lpf_NE * alpha + corrected_delta_vel_ef.xy();
// Calculate low pass filtered height rate
float alpha_height_rate_lpf = 0.1f * imu_delayed.delta_vel_dt; // 10 seconds time constant
_height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf;
// calculate a filtered horizontal acceleration this are used for manoeuvre detection elsewhere
_accel_horiz_lpf.update(corrected_delta_vel_ef.xy() / imu_delayed.delta_vel_dt, imu_delayed.delta_vel_dt);
}
bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const double longitude, const float altitude,

9
src/modules/ekf2/EKF/ekf.h
View File

@ -490,8 +490,13 @@ private:
Dcmf _R_to_earth{}; ///< transformation matrix from body frame to earth frame from last EKF prediction
Vector2f _accel_lpf_NE{}; ///< Low pass filtered horizontal earth frame acceleration (m/sec**2)
float _height_rate_lpf{0.0f};
static constexpr float _kAccelHorizLpfTimeConstant = 1.f;
AlphaFilter<Vector2f> _accel_horiz_lpf{_kAccelHorizLpfTimeConstant}; ///< Low pass filtered horizontal earth frame acceleration (m/sec**2)
#if defined(CONFIG_EKF2_WIND)
static constexpr float _kHeightRateLpfTimeConstant = 10.f;
AlphaFilter<float> _height_rate_lpf{_kHeightRateLpfTimeConstant};
#endif // CONFIG_EKF2_WIND
SquareMatrixState P{}; ///< state covariance matrix