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

ekf2: new EKF2_IMU_CTRL parameter and gyro bias inhibit mechanism

Browse Source 
- EKF2_AID_MASK accel bias inhibit moves to EKF2_IMU_CTRL
This commit is contained in:
Daniel Agar
2023-02-03 09:52:24 -05:00
committed by GitHub
parent f668ea5aa6
commit 264a99fb77
7 changed files with 112 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.h
+10 -1
View File
@@ -329,6 +329,7 @@ public:
float getMagBiasLimit() const { return 0.5f; } // 0.5 Gauss
bool accel_bias_inhibited() const { return _accel_bias_inhibit[0] || _accel_bias_inhibit[1] || _accel_bias_inhibit[2]; }
bool gyro_bias_inhibited() const { return _gyro_bias_inhibit[0] || _gyro_bias_inhibit[1] || _gyro_bias_inhibit[2]; }
const auto &state_reset_status() const { return _state_reset_status; }
@@ -531,8 +532,8 @@ private:
SquareMatrix24f P{}; ///< state covariance matrix
Vector3f _delta_vel_bias_var_accum{}; ///< kahan summation algorithm accumulator for delta velocity bias variance
Vector3f _delta_angle_bias_var_accum{}; ///< kahan summation algorithm accumulator for delta angle bias variance
Vector3f _delta_vel_bias_var_accum{}; ///< kahan summation algorithm accumulator for delta velocity bias variance
Vector2f _drag_innov{}; ///< multirotor drag measurement innovation (m/sec**2)
Vector2f _drag_innov_var{}; ///< multirotor drag measurement innovation variance ((m/sec**2)**2)
@@ -614,9 +615,11 @@ 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)
Vector3f _prev_delta_ang_bias_var{}; ///< saved delta angle XYZ bias variances (rad/sec)
Vector3f _prev_dvel_bias_var{}; ///< saved delta velocity XYZ bias variances (m/sec)**2
// Terrain height state estimation
@@ -765,6 +768,12 @@ private:
bool measurementUpdate(Vector24f &K, float innovation_variance, float innovation)
{
for (unsigned i = 0; i < 3; i++) {
// gyro bias: states 10, 11, 12
if (_gyro_bias_inhibit[i]) {
K(10 + i) = 0.0f;
}
// accel bias: states 13, 14, 15
if (_accel_bias_inhibit[i]) {
K(13 + i) = 0.0f;
}