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

PX4 style fixes (excluding EKF generated code)

Browse Source
This commit is contained in:
Daniel Agar
2018-05-03 22:25:36 -04:00
parent e5952fadaf
commit c5b2f59f57
22 changed files with 384 additions and 187 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/covariance.cpp
+50 -29
View File
@@ -72,12 +72,15 @@ void Ekf::initialiseCovariance()
// position
P[7][7] = sq(fmaxf(_params.gps_pos_noise, 0.01f));
P[8][8] = P[7][7];
if (_control_status.flags.rng_hgt) {
P[9][9] = sq(fmaxf(_params.range_noise, 0.01f));
} else if (_control_status.flags.gps_hgt) {
float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
P[9][9] = sq(1.5f * math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit));
} else {
P[9][9] = sq(fmaxf(_params.baro_noise, 0.01f));
}
@@ -165,32 +168,38 @@ void Ekf::predictCovariance()
_accel_mag_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_vel.norm(), alpha * _accel_mag_filt);
if (_ang_rate_mag_filt > _params.acc_bias_learn_gyr_lim
|| _accel_mag_filt > _params.acc_bias_learn_acc_lim
|| _bad_vert_accel_detected) {
|| _accel_mag_filt > _params.acc_bias_learn_acc_lim
|| _bad_vert_accel_detected) {
// store the bias state variances to be reinstated later
if (!_accel_bias_inhibit) {
_prev_dvel_bias_var(0) = P[13][13];
_prev_dvel_bias_var(1) = P[14][14];
_prev_dvel_bias_var(2) = P[15][15];
}
_accel_bias_inhibit = true;
} else {
if (_accel_bias_inhibit) {
// reinstate the bias state variances
P[13][13] = _prev_dvel_bias_var(0);
P[14][14] = _prev_dvel_bias_var(1);
P[15][15] = _prev_dvel_bias_var(2);
} else {
// store the bias state variances to be reinstated later
_prev_dvel_bias_var(0) = P[13][13];
_prev_dvel_bias_var(1) = P[14][14];
_prev_dvel_bias_var(2) = P[15][15];
}
_accel_bias_inhibit = false;
}
// Don't continue to grow the earth field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
float mag_I_sig;
if (_control_status.flags.mag_3D && (P[16][16] + P[17][17] + P[18][18]) < 0.1f) {
mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.0f, 1.0f);
@@ -200,6 +209,7 @@ void Ekf::predictCovariance()
// Don't continue to grow the body field variances if they is becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
float mag_B_sig;
if (_control_status.flags.mag_3D && (P[19][19] + P[20][20] + P[21][21]) < 0.1f) {
mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.0f, 1.0f);
@@ -222,6 +232,7 @@ void Ekf::predictCovariance()
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
@@ -240,11 +251,13 @@ void Ekf::predictCovariance()
float gyro_noise = math::constrain(_params.gyro_noise, 0.0f, 1.0f);
daxVar = dayVar = dazVar = sq(dt * gyro_noise);
float accel_noise = math::constrain(_params.accel_noise, 0.0f, 1.0f);
if (_bad_vert_accel_detected) {
// Increase accelerometer process noise if bad accel data is detected. Measurement errors due to
// vibration induced clipping commonly reach an equivalent 0.5g offset.
accel_noise = BADACC_BIAS_PNOISE;
}
dvxVar = dvyVar = dvzVar = sq(dt * accel_noise);
// predict the covariance
@@ -467,8 +480,8 @@ void Ekf::predictCovariance()
} else {
// Inhibit delta velocity bias learning by zeroing the covariance terms
zeroRows(nextP,13,15);
zeroCols(nextP,13,15);
zeroRows(nextP, 13, 15);
zeroCols(nextP, 13, 15);
}
// Don't do covariance prediction on magnetic field states unless we are using 3-axis fusion
@@ -725,15 +738,16 @@ void Ekf::fixCovarianceErrors()
}
// force symmetry on the quaternion, velocity and positon state covariances
makeSymmetrical(P,0,12);
makeSymmetrical(P, 0, 12);
// the following states are optional and are deactivaed when not required
// by ensuring the corresponding covariance matrix values are kept at zero
// accelerometer bias states
if ((_params.fusion_mode & MASK_INHIBIT_ACC_BIAS) || _accel_bias_inhibit) {
zeroRows(P,13,15);
zeroCols(P,13,15);
zeroRows(P, 13, 15);
zeroCols(P, 13, 15);
} else {
// Find the maximum delta velocity bias state variance and request a covariance reset if any variance is below the safe minimum
const float minSafeStateVar = 1e-9f;
@@ -795,6 +809,7 @@ void Ekf::fixCovarianceErrors()
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;
}
@@ -805,63 +820,69 @@ void Ekf::fixCovarianceErrors()
float varX = P[13][13];
float varY = P[14][14];
float varZ = P[15][15];
zeroRows(P,13,15);
zeroCols(P,13,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);
makeSymmetrical(P, 13, 15);
}
}
// magnetic field states
if (!_control_status.flags.mag_3D) {
zeroRows(P,16,21);
zeroCols(P,16,21);
zeroRows(P, 16, 21);
zeroCols(P, 16, 21);
} else {
// constrain variances
for (int i = 16; i <= 18; i++) {
P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[5]);
}
for (int i = 19; i <= 21; i++) {
P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[6]);
}
// force symmetry
makeSymmetrical(P,16,21);
makeSymmetrical(P, 16, 21);
}
// wind velocity states
if (!_control_status.flags.wind) {
zeroRows(P,22,23);
zeroCols(P,22,23);
zeroRows(P, 22, 23);
zeroCols(P, 22, 23);
} else {
// constrain variances
for (int i = 22; i <= 23; i++) {
P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[7]);
}
// force symmetry
makeSymmetrical(P,22,23);
makeSymmetrical(P, 22, 23);
}
}
void Ekf::resetMagCovariance()
{
// set the quaternion covariance terms to zero
zeroRows(P,0,3);
zeroCols(P,0,3);
zeroRows(P, 0, 3);
zeroCols(P, 0, 3);
// set the magnetic field covariance terms to zero
zeroRows(P,16,21);
zeroCols(P,16,21);
zeroRows(P, 16, 21);
zeroCols(P, 16, 21);
// set the field state variance to the observation variance
for (uint8_t rc_index=16; rc_index <= 21; rc_index ++) {
for (uint8_t rc_index = 16; rc_index <= 21; rc_index ++) {
P[rc_index][rc_index] = sq(_params.mag_noise);
}
}
@@ -869,15 +890,15 @@ void Ekf::resetMagCovariance()
void Ekf::resetWindCovariance()
{
// set the wind covariance terms to zero
zeroRows(P,22,23);
zeroCols(P,22,23);
zeroRows(P, 22, 23);
zeroCols(P, 22, 23);
if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < (uint64_t)5e5)) {
// Use airspeed and zer sideslip assumption to set initial covariance values for wind states
// calculate the wind speed and bearing
float spd = sqrtf(sq(_state.wind_vel(0))+sq(_state.wind_vel(1)));
float yaw = atan2f(_state.wind_vel(1),_state.wind_vel(0));
float spd = sqrtf(sq(_state.wind_vel(0)) + sq(_state.wind_vel(1)));
float yaw = atan2f(_state.wind_vel(1), _state.wind_vel(0));
// calculate the uncertainty in wind speed and direction using the uncertainty in airspeed and sideslip angle
// used to calculate the initial wind speed
@@ -889,12 +910,12 @@ void Ekf::resetWindCovariance()
float sin_yaw = sinf(yaw);
float cos_yaw_2 = sq(cos_yaw);
float sin_yaw_2 = sq(sin_yaw);
float sin_cos_yaw = sin_yaw*cos_yaw;
float sin_cos_yaw = sin_yaw * cos_yaw;
float spd_2 = sq(spd);
P[22][22] = R_yaw*spd_2*sin_yaw_2 + R_spd*cos_yaw_2;
P[22][23] = - R_yaw*sin_cos_yaw*spd_2 + R_spd*sin_cos_yaw;
P[22][22] = R_yaw * spd_2 * sin_yaw_2 + R_spd * cos_yaw_2;
P[22][23] = - R_yaw * sin_cos_yaw * spd_2 + R_spd * sin_cos_yaw;
P[23][22] = P[22][23];
P[23][23] = R_yaw*spd_2*cos_yaw_2 + R_spd*sin_yaw_2;
P[23][23] = R_yaw * spd_2 * cos_yaw_2 + R_spd * sin_yaw_2;
// Now add the variance due to uncertainty in vehicle velocity that was used to calculate the initial wind speed
P[22][22] += P[4][4];