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

Revert "EKF: Add parameter control of individual IMU axis delta velocity bias estimation"

Browse Source 
This reverts commit 9c31632e2b.
This commit is contained in:
Paul Riseborough
2017-05-15 16:38:17 +10:00
parent 47973a4820
commit 20db74eca8
3 changed files with 58 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/common.h
-2
View File
@@ -246,7 +246,6 @@ struct parameters {
float rng_gnd_clearance{0.1f}; // minimum valid value for range when on ground (m)
float rng_sens_pitch{0.0f}; // Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis.
float range_noise_scaler{0.0f}; // scaling from range measurement to noise (m/m)
float vehicle_variance_scaler{0.0f}; // gain applied to vehicle height variance used in calculation of height above ground observation variance
// vision position fusion
float ev_innov_gate{5.0f}; // vision estimator fusion innovation consistency gate size (STD)
@@ -285,7 +284,6 @@ struct parameters {
float acc_bias_learn_acc_lim{25.0f}; // learning is disabled if the magnitude of the IMU acceleration vector is greeater than this (m/sec**2)
float acc_bias_learn_gyr_lim{3.0f}; // learning is disabled if the magnitude of the IMU angular rate vector is greeater than this (rad/sec)
float acc_bias_learn_tc{0.5f}; // time constant used to control the decaying envelope filters applied to the accel and gyro magnitudes (sec)
int acc_bias_learn_mask{7}; // bitmask used to control which accelerometer axes learn an in-flight bias
unsigned no_gps_timeout_max{7000000}; // maximum time we allow dead reckoning while both gps position and velocity measurements are being
// rejected before attempting to reset the states to the GPS measurement (usec)
EKF/covariance.cpp
+56 -82
View File
@@ -47,7 +47,11 @@
void Ekf::initialiseCovariance()
{
memset(P, 0, sizeof(P));
for (unsigned i = 0; i < _k_num_states; i++) {
for (unsigned j = 0; j < _k_num_states; j++) {
P[i][j] = 0.0f;
}
}
// calculate average prediction time step in sec
float dt = 0.001f * (float)FILTER_UPDATE_PERIOD_MS;
@@ -83,16 +87,9 @@ void Ekf::initialiseCovariance()
P[12][12] = P[10][10];
// accel bias
float temp_var = sq(_params.switch_on_accel_bias * dt);
if (_params.acc_bias_learn_mask & (1<<0)) {
_prev_dvel_bias_var(0) = P[13][13] = temp_var;
}
if (_params.acc_bias_learn_mask & (1<<1)) {
_prev_dvel_bias_var(1) = P[14][14] = temp_var;
}
if (_params.acc_bias_learn_mask & (1<<2)) {
_prev_dvel_bias_var(2) = P[15][15] = temp_var;
}
_prev_dvel_bias_var(0) = P[13][13] = sq(_params.switch_on_accel_bias * dt);
_prev_dvel_bias_var(1) = P[14][14] = P[13][13];
_prev_dvel_bias_var(2) = P[15][15] = P[13][13];
// variances for optional states
@@ -214,10 +211,11 @@ void Ekf::predictCovariance()
wind_vel_sig = 0.0f;
}
// Construct the process noise variance diagonal for those states with a stationary process model.
// The covariance growth of the quaternion, velocity and position states is controlled separately
// by the IMU noise variance terms built into the auto generated covariance prediction code.
// Construct the process noise variance diagonal for those states with a stationary process model
// These are kinematic states and their error growth is controlled separately by the IMU noise variances
for (unsigned i = 0; i <= 9; i++) {
process_noise[i] = 0.0f;
}
// delta angle bias states
process_noise[12] = process_noise[11] = process_noise[10] = sq(d_ang_bias_sig);
// delta_velocity bias states
@@ -401,8 +399,8 @@ void Ekf::predictCovariance()
nextP[11][12] = P[11][12];
nextP[12][12] = P[12][12];
// add gyro bias process noise variance
for (unsigned i = 10; i <= 12; i++) {
// add process noise that is not from the IMU
for (unsigned i = 0; i <= 12; i++) {
nextP[i][i] += process_noise[i];
}
@@ -423,13 +421,7 @@ void Ekf::predictCovariance()
nextP[10][13] = P[10][13];
nextP[11][13] = P[11][13];
nextP[12][13] = P[12][13];
if (_params.acc_bias_learn_mask & (1<<0)) {
nextP[13][13] = P[13][13] + process_noise[13];
} else {
nextP[13][13] = 0.0f;
}
nextP[13][13] = P[13][13];
nextP[0][14] = P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10];
nextP[1][14] = P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2;
nextP[2][14] = P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2;
@@ -444,13 +436,7 @@ void Ekf::predictCovariance()
nextP[11][14] = P[11][14];
nextP[12][14] = P[12][14];
nextP[13][14] = P[13][14];
if (_params.acc_bias_learn_mask & (1<<1)) {
nextP[14][14] = P[14][14] + process_noise[14];
} else {
nextP[14][14] = 0.0f;
}
nextP[14][14] = P[14][14];
nextP[0][15] = P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10];
nextP[1][15] = P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2;
nextP[2][15] = P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2;
@@ -466,11 +452,11 @@ void Ekf::predictCovariance()
nextP[12][15] = P[12][15];
nextP[13][15] = P[13][15];
nextP[14][15] = P[14][15];
nextP[15][15] = P[15][15];
if (_params.acc_bias_learn_mask & (1<<2)) {
nextP[15][15] = P[15][15] + process_noise[15];
} else {
nextP[15][15] = 0.0f;
// add process noise that is not from the IMU
for (unsigned i = 13; i <= 15; i++) {
nextP[i][i] += process_noise[i];
}
} else {
@@ -607,7 +593,7 @@ void Ekf::predictCovariance()
nextP[20][21] = P[20][21];
nextP[21][21] = P[21][21];
// add magnetic field state process noise variance
// add process noise that is not from the IMU
for (unsigned i = 16; i <= 21; i++) {
nextP[i][i] += process_noise[i];
}
@@ -666,7 +652,7 @@ void Ekf::predictCovariance()
nextP[22][23] = P[22][23];
nextP[23][23] = P[23][23];
// add wind state process noise variance
// add process noise that is not from the IMU
for (unsigned i = 22; i <= 23; i++) {
nextP[i][i] += process_noise[i];
}
@@ -751,56 +737,44 @@ void Ekf::fixCovarianceErrors()
zeroCols(P,13,15);
} else {
// constrain variances
for (uint8_t i=0; i < 3; i++) {
uint8_t state_index = 13 + i;
if (_params.acc_bias_learn_mask & (1<<i)) {
P[state_index][state_index] = math::constrain(P[state_index][state_index], 0.0f, P_lim[4]);
} else {
P[state_index][state_index] = 0.0f;
}
for (int i = 13; i <= 15; i++) {
P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[4]);
}
// if we are estimating three axes, check for poor numerical conditioning and reset covariances if necessary
if (_params.acc_bias_learn_mask == 7) {
// calculate accel bias term aligned with the gravity vector
float dVel_bias_lim = 0.9f * _params.acc_bias_lim * _dt_ekf_avg;
float down_dvel_bias = 0.0f;
for (uint8_t axis_index=0; axis_index < 3; axis_index++) {
down_dvel_bias += _state.accel_bias(axis_index) * _R_to_earth(2,axis_index);
}
// calculate accel bias term aligned with the gravity vector
float dVel_bias_lim = 0.9f * _params.acc_bias_lim * _dt_ekf_avg;
float down_dvel_bias = 0.0f;
for (uint8_t axis_index=0; axis_index < 3; axis_index++) {
down_dvel_bias += _state.accel_bias(axis_index) * _R_to_earth(2,axis_index);
}
// check that the vertical componenent of accel bias is consistent with both the vertical position and velocity innovation
bool bad_acc_bias = (fabsf(down_dvel_bias) > dVel_bias_lim
&& down_dvel_bias * _vel_pos_innov[2] < 0.0f
&& down_dvel_bias * _vel_pos_innov[5] < 0.0f);
// check that the vertical componenent of accel bias is consistent with both the vertical position and velocity innovation
bool bad_acc_bias = (fabsf(down_dvel_bias) > dVel_bias_lim
&& down_dvel_bias * _vel_pos_innov[2] < 0.0f
&& down_dvel_bias * _vel_pos_innov[5] < 0.0f);
// record the pass/fail
if (!bad_acc_bias) {
_fault_status.flags.bad_acc_bias = false;
_time_acc_bias_check = _time_last_imu;
} else {
_fault_status.flags.bad_acc_bias = true;
}
// if we have failed for 7 seconds continuously, reset the accel bias covariances to fix bad conditioning of
// the covariance matrix but preserve the variances (diagonals) to allow bias learning to continue
if (_time_last_imu - _time_acc_bias_check > 7E6) {
float varX = P[13][13];
float varY = P[14][14];
float varZ = P[15][15];
zeroRows(P,13,15);
zeroCols(P,13,15);
P[13][13] = varX;
P[14][14] = varY;
P[15][15] = varZ;
_time_acc_bias_check = _time_last_imu;
_fault_status.flags.bad_acc_bias = false;
ECL_WARN("EKF invalid accel bias - resetting covariance");
} else {
// ensure the covariance values are symmetrical
makeSymmetrical(P,13,15);
}
// record the pass/fail
if (!bad_acc_bias) {
_fault_status.flags.bad_acc_bias = false;
_time_acc_bias_check = _time_last_imu;
} else {
_fault_status.flags.bad_acc_bias = true;
}
// if we have failed for 7 seconds continuously, reset the accel bias covariances to fix bad conditioning of
// the covariance matrix but preserve the variances (diagonals) to allow bias learning to continue
if (_time_last_imu - _time_acc_bias_check > 7E6) {
float varX = P[13][13];
float varY = P[14][14];
float varZ = P[15][15];
zeroRows(P,13,15);
zeroCols(P,13,15);
P[13][13] = varX;
P[14][14] = varY;
P[15][15] = varZ;
_time_acc_bias_check = _time_last_imu;
_fault_status.flags.bad_acc_bias = false;
ECL_WARN("EKF invalid accel bias - resetting covariance");
} else {
// ensure the covariance values are symmetrical
makeSymmetrical(P,13,15);
EKF/ekf_helper.cpp
+2 -8
View File
@@ -549,14 +549,8 @@ void Ekf::constrainStates()
_state.gyro_bias(i) = math::constrain(_state.gyro_bias(i), -0.349066f * _dt_ekf_avg, 0.349066f * _dt_ekf_avg);
}
float del_vel_bias_lim = _params.acc_bias_lim * _dt_ekf_avg;
for (uint8_t i=0; i < 3; i++) {
uint8_t state_index = 13 + i;
if (_params.acc_bias_learn_mask & (1<<i)) {
_state.accel_bias(state_index) = math::constrain(_state.accel_bias(state_index), -del_vel_bias_lim, del_vel_bias_lim);
} else {
_state.accel_bias(state_index) = 0.0f;
}
for (int i = 0; i < 3; i++) {
_state.accel_bias(i) = math::constrain(_state.accel_bias(i), -_params.acc_bias_lim * _dt_ekf_avg, _params.acc_bias_lim * _dt_ekf_avg);
}
for (int i = 0; i < 3; i++) {