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

ekf2: extend sensor bias stability requirement

Browse Source 
- for accel/gyro/mag estimated bias only consider them stable (valid
for calibration updates) if the value isn't changing (10% of limit) over
the validity period
This commit is contained in:
Daniel Agar
2022-09-28 10:40:52 -04:00
parent 16f8adb4b3
commit 1f7080a710
4 changed files with 84 additions and 100 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.h
+10 -4
View File
@@ -298,13 +298,18 @@ public:
// get the terrain variance
float get_terrain_var() const { return _terrain_var; }
// gyro bias (states 10, 11, 12)
Vector3f getGyroBias() const { return _state.delta_ang_bias / _dt_ekf_avg; } // get the gyroscope bias in rad/s
Vector3f getAccelBias() const { return _state.delta_vel_bias / _dt_ekf_avg; } // get the accelerometer bias in m/s**2
const Vector3f &getMagBias() const { return _state.mag_B; }
Vector3f getGyroBiasVariance() const { return Vector3f{P(10, 10), P(11, 11), P(12, 12)} / sq(_dt_ekf_avg); } // get the gyroscope bias variance in rad/s
Vector3f getAccelBiasVariance() const { return Vector3f{P(13, 13), P(14, 14), P(15, 15)} / sq(_dt_ekf_avg); } // get the accelerometer bias variance in m/s**2
float getGyroBiasLimit() const { return math::radians(20.f); } // 20 degrees/s
// accel bias (states 13, 14, 15)
Vector3f getAccelBias() const { return _state.delta_vel_bias / _dt_ekf_avg; } // get the accelerometer bias in m/s**2
Vector3f getAccelBiasVariance() const { return Vector3f{P(13, 13), P(14, 14), P(15, 15)} / sq(_dt_ekf_avg); } // get the accelerometer bias variance in m/s**2
float getAccelBiasLimit() const { return _params.acc_bias_lim; }
// mag bias (states 19, 20, 21)
const Vector3f &getMagBias() const { return _state.mag_B; }
Vector3f getMagBiasVariance() const
{
if (_control_status.flags.mag_3D) {
@@ -313,6 +318,7 @@ public:
return _saved_mag_bf_variance;
}
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]; }
src/modules/ekf2/EKF/ekf_helper.cpp
+3 -3
View File
@@ -501,14 +501,14 @@ void Ekf::constrainStates()
_state.vel = matrix::constrain(_state.vel, -1000.0f, 1000.0f);
_state.pos = matrix::constrain(_state.pos, -1.e6f, 1.e6f);
const float delta_ang_bias_limit = math::radians(20.f) * _dt_ekf_avg;
const float delta_ang_bias_limit = getGyroBiasLimit() * _dt_ekf_avg;
_state.delta_ang_bias = matrix::constrain(_state.delta_ang_bias, -delta_ang_bias_limit, delta_ang_bias_limit);
const float delta_vel_bias_limit = _params.acc_bias_lim * _dt_ekf_avg;
const float delta_vel_bias_limit = getAccelBiasLimit() * _dt_ekf_avg;
_state.delta_vel_bias = matrix::constrain(_state.delta_vel_bias, -delta_vel_bias_limit, delta_vel_bias_limit);
_state.mag_I = matrix::constrain(_state.mag_I, -1.0f, 1.0f);
_state.mag_B = matrix::constrain(_state.mag_B, -0.5f, 0.5f);
_state.mag_B = matrix::constrain(_state.mag_B, -getMagBiasLimit(), getMagBiasLimit());
_state.wind_vel = matrix::constrain(_state.wind_vel, -100.0f, 100.0f);
}