dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2-grav: only use filtered accel norm to start/stop the fusion 

Using the raw data makes the swith too sensitive to noise
This commit is contained in:
bresch 2024-01-18 15:56:01 +01:00 committed by Daniel Agar
parent 2b69a3d290
commit c28972d15e
3 changed files with 2 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -727,7 +727,6 @@ private:
// variables used to inhibit accel bias learning
bool _accel_bias_inhibit[3] {}; ///< true when the accel bias learning is being inhibited for the specified axis
bool _gyro_bias_inhibit[3] {}; ///< true when the gyro bias learning is being inhibited for the specified axis
Vector3f _accel_vec_filt{}; ///< acceleration vector after application of a low pass filter (m/sec**2)
float _accel_magnitude_filt{0.0f}; ///< acceleration magnitude after application of a decaying envelope filter (rad/sec)
float _ang_rate_magnitude_filt{0.0f}; ///< angular rate magnitude after application of a decaying envelope filter (rad/sec)

1
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -987,7 +987,6 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
const float beta = 1.f - alpha;
_accel_magnitude_filt = fmaxf(imu_delayed.delta_vel.norm() / imu_delayed.delta_vel_dt, beta * _accel_magnitude_filt);
_accel_vec_filt = alpha * imu_delayed.delta_vel / imu_delayed.delta_vel_dt + beta * _accel_vec_filt;
}

7
src/modules/ekf2/EKF/gravity_fusion.cpp
View File

@ -50,16 +50,13 @@ void Ekf::controlGravityFusion(const imuSample &imu)
const Vector3f measurement = imu.delta_vel / imu.delta_vel_dt - _state.accel_bias;
const float measurement_var = math::max(sq(_params.gravity_noise), sq(0.01f));
const float accel_lpf_norm_sq = _accel_vec_filt.norm_squared();
const float accel_norm_sq = measurement.norm_squared();
const float upper_accel_limit = CONSTANTS_ONE_G * 1.1f;
const float lower_accel_limit = CONSTANTS_ONE_G * 0.9f;
const bool accel_lpf_norm_good = (accel_lpf_norm_sq > sq(lower_accel_limit)) && (accel_lpf_norm_sq < sq(upper_accel_limit));
const bool accel_norm_good = (accel_norm_sq > sq(lower_accel_limit)) && (accel_norm_sq < sq(upper_accel_limit));
const bool accel_lpf_norm_good = (_accel_magnitude_filt > lower_accel_limit) && (_accel_magnitude_filt < upper_accel_limit);
// fuse gravity observation if our overall acceleration isn't too big
_control_status.flags.gravity_vector = (_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector))
&& ((accel_lpf_norm_good && accel_norm_good) || _control_status.flags.vehicle_at_rest)
&& (accel_lpf_norm_good || _control_status.flags.vehicle_at_rest)
&& !isHorizontalAidingActive();
// calculate kalman gains and innovation variances