fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 14:03: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/common.h
+8 -1
View File
@@ -126,6 +126,11 @@ enum class PositionSensor : uint8_t {
EV = 2,
};
enum class ImuCtrl : uint8_t {
GyroBias = (1<<0),
AccelBias = (1<<1),
};
enum GnssCtrl : uint8_t {
HPOS = (1<<0),
VPOS = (1<<1),
@@ -150,7 +155,7 @@ enum SensorFusionMask : uint16_t {
// Bit locations for fusion_mode
DEPRECATED_USE_GPS = (1<<0), ///< set to true to use GPS data (DEPRECATED, use gnss_ctrl)
USE_OPT_FLOW = (1<<1), ///< set to true to use optical flow data
INHIBIT_ACC_BIAS = (1<<2), ///< set to true to inhibit estimation of accelerometer delta velocity bias
DEPRECATED_INHIBIT_ACC_BIAS = (1<<2), ///< set to true to inhibit estimation of accelerometer delta velocity bias
DEPRECATED_USE_EXT_VIS_POS = (1<<3), ///< set to true to use external vision position data
DEPRECATED_USE_EXT_VIS_YAW = (1<<4), ///< set to true to use external vision quaternion data for yaw
USE_DRAG = (1<<5), ///< set to true to use the multi-rotor drag model to estimate wind
@@ -279,6 +284,8 @@ struct parameters {
int32_t filter_update_interval_us{10000}; ///< filter update interval in microseconds
int32_t imu_ctrl{static_cast<int32_t>(ImuCtrl::GyroBias) | static_cast<int32_t>(ImuCtrl::AccelBias)};
// measurement source control
int32_t fusion_mode{}; ///< bitmasked integer that selects some aiding sources
int32_t height_sensor_ref{HeightSensor::BARO};
src/modules/ekf2/EKF/covariance.cpp
+45 -6
View File
@@ -81,9 +81,9 @@ void Ekf::initialiseCovariance()
}
// gyro bias
P(10,10) = sq(_params.switch_on_gyro_bias * dt);
P(11,11) = P(10,10);
P(12,12) = P(10,10);
_prev_delta_ang_bias_var(0) = P(10,10) = sq(_params.switch_on_gyro_bias * dt);
_prev_delta_ang_bias_var(1) = P(11,11) = P(10,10);
_prev_delta_ang_bias_var(2) = P(12,12) = P(10,10);
// accel bias
_prev_dvel_bias_var(0) = P(13,13) = sq(_params.switch_on_accel_bias * dt);
@@ -121,7 +121,36 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
const bool is_manoeuvre_level_high = _ang_rate_magnitude_filt > _params.acc_bias_learn_gyr_lim
|| _accel_magnitude_filt > _params.acc_bias_learn_acc_lim;
const bool do_inhibit_all_axes = (_params.fusion_mode & SensorFusionMask::INHIBIT_ACC_BIAS)
// gyro bias inhibit
const bool do_inhibit_all_gyro_axes = !(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GyroBias));
for (unsigned stateIndex = 10; stateIndex <= 12; stateIndex++) {
const unsigned index = stateIndex - 10;
bool is_bias_observable = true;
// TODO: gyro bias conditions
const bool do_inhibit_axis = do_inhibit_all_gyro_axes || !is_bias_observable;
if (do_inhibit_axis) {
// store the bias state variances to be reinstated later
if (!_gyro_bias_inhibit[index]) {
_prev_delta_ang_bias_var(index) = P(stateIndex, stateIndex);
_gyro_bias_inhibit[index] = true;
}
} else {
if (_gyro_bias_inhibit[index]) {
// reinstate the bias state variances
P(stateIndex, stateIndex) = _prev_delta_ang_bias_var(index);
_gyro_bias_inhibit[index] = false;
}
}
}
// accel bias inhibit
const bool do_inhibit_all_accel_axes = !(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::AccelBias))
|| is_manoeuvre_level_high
|| _fault_status.flags.bad_acc_vertical;
@@ -141,7 +170,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
is_bias_observable = (fabsf(_R_to_earth(2, index)) > 0.966f); // cos 15 degrees ~= 0.966
}
const bool do_inhibit_axis = do_inhibit_all_axes || imu_delayed.delta_vel_clipping[index] || !is_bias_observable;
const bool do_inhibit_axis = do_inhibit_all_accel_axes || imu_delayed.delta_vel_clipping[index] || !is_bias_observable;
if (do_inhibit_axis) {
// store the bias state variances to be reinstated later
@@ -239,7 +268,17 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
// the variances, therefore use algorithm to minimise numerical error
for (unsigned i = 10; i <= 12; i++) {
const int index = i - 10;
nextP(i, i) = kahanSummation(nextP(i, i), process_noise(i), _delta_angle_bias_var_accum(index));
if (!_gyro_bias_inhibit[index]) {
// add process noise that is not from the IMU
// process noise contribution for delta velocity states can be very small compared to
// the variances, therefore use algorithm to minimise numerical error
nextP(i, i) = kahanSummation(nextP(i, i), process_noise(i), _delta_angle_bias_var_accum(index));
} else {
nextP.uncorrelateCovarianceSetVariance<1>(i, _prev_delta_ang_bias_var(index));
_delta_angle_bias_var_accum(index) = 0.f;
}
}
for (int i = 13; i <= 15; i++) {
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;
}
src/modules/ekf2/EKF/vel_pos_fusion.cpp
+6
View File
@@ -201,6 +201,12 @@ bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o
}
for (unsigned i = 0; i < 3; i++) {
// gyro bias: states 10, 11, 12
if (_gyro_bias_inhibit[i]) {
Kfusion(10 + i) = 0.0f;
}
// accel bias: states 13, 14, 15
if (_accel_bias_inhibit[i]) {
Kfusion(13 + i) = 0.0f;
}