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

ekf2: access state covariance using enum

Browse Source
This commit is contained in:
bresch
2023-09-19 09:37:50 -04:00
committed by Daniel Agar
parent 9c41c06325
commit 779ea3f4d1
15 changed files with 256 additions and 181 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/covariance.cpp
+114 -97
View File
@@ -57,41 +57,39 @@ void Ekf::initialiseCovariance()
resetQuatCov();
// velocity
P(4,4) = sq(fmaxf(_params.gps_vel_noise, 0.01f));
P(5,5) = P(4,4);
P(6,6) = sq(1.5f) * P(4,4);
const float vel_var = sq(fmaxf(_params.gps_vel_noise, 0.01f));
P.uncorrelateCovarianceSetVariance<State::vel.dof>(State::vel.idx, Vector3f(vel_var, vel_var, sq(1.5f) * vel_var));
// position
P(7,7) = sq(fmaxf(_params.gps_pos_noise, 0.01f));
P(8,8) = P(7,7);
P(9,9) = sq(fmaxf(_params.baro_noise, 0.01f));
const float xy_pos_var = sq(fmaxf(_params.gps_pos_noise, 0.01f));
float z_pos_var = sq(fmaxf(_params.baro_noise, 0.01f));
if (_control_status.flags.gps_hgt) {
P(9,9) = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f));
z_pos_var = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f));
}
#if defined(CONFIG_EKF2_RANGE_FINDER)
if (_control_status.flags.rng_hgt) {
P(9,9) = sq(fmaxf(_params.range_noise, 0.01f));
z_pos_var = sq(fmaxf(_params.range_noise, 0.01f));
}
#endif // CONFIG_EKF2_RANGE_FINDER
P.uncorrelateCovarianceSetVariance<State::pos.dof>(State::pos.idx, Vector3f(xy_pos_var, xy_pos_var, z_pos_var));
// gyro bias
_prev_gyro_bias_var(0) = P(10,10) = sq(_params.switch_on_gyro_bias);
_prev_gyro_bias_var(1) = P(11,11) = P(10,10);
_prev_gyro_bias_var(2) = P(12,12) = P(10,10);
const float gyro_bias_var = sq(_params.switch_on_gyro_bias);
P.uncorrelateCovarianceSetVariance<State::gyro_bias.dof>(State::gyro_bias.idx, gyro_bias_var);
_prev_gyro_bias_var.setAll(gyro_bias_var);
// accel bias
_prev_accel_bias_var(0) = P(13,13) = sq(_params.switch_on_accel_bias);
_prev_accel_bias_var(1) = P(14,14) = P(13,13);
_prev_accel_bias_var(2) = P(15,15) = P(13,13);
const float accel_bias_var = sq(_params.switch_on_accel_bias);
P.uncorrelateCovarianceSetVariance<State::accel_bias.dof>(State::accel_bias.idx, accel_bias_var);
_prev_accel_bias_var.setAll(accel_bias_var);
resetMagCov();
// wind
P(22,22) = sq(_params.initial_wind_uncertainty);
P(23,23) = P(22,22);
P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, sq(_params.initial_wind_uncertainty));
}
void Ekf::predictCovariance(const imuSample &imu_delayed)
@@ -113,8 +111,8 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
// 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;
for (unsigned index = 0; index < State::gyro_bias.dof; index++) {
const unsigned stateIndex = State::gyro_bias.idx + index;
bool is_bias_observable = true;
@@ -143,8 +141,8 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
|| is_manoeuvre_level_high
|| _fault_status.flags.bad_acc_vertical;
for (unsigned stateIndex = 13; stateIndex <= 15; stateIndex++) {
const unsigned index = stateIndex - 13;
for (unsigned index = 0; index < State::accel_bias.dof; index++) {
const unsigned stateIndex = State::accel_bias.idx + index;
bool is_bias_observable = true;
@@ -180,7 +178,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
// 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 = 0.0f;
if (_control_status.flags.mag && (P(16, 16) + P(17, 17) + P(18, 18)) < 0.1f) {
if (_control_status.flags.mag && P.trace<State::mag_I.dof>(State::mag_I.idx) < 0.1f) {
#if defined(CONFIG_EKF2_MAGNETOMETER)
mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.0f, 1.0f);
#endif // CONFIG_EKF2_MAGNETOMETER
@@ -189,7 +187,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
// 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 = 0.0f;
if (_control_status.flags.mag && (P(19, 19) + P(20, 20) + P(21, 21)) < 0.1f) {
if (_control_status.flags.mag && P.trace<State::mag_B.dof>(State::mag_B.idx) < 0.1f) {
#if defined(CONFIG_EKF2_MAGNETOMETER)
mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.0f, 1.0f);
#endif // CONFIG_EKF2_MAGNETOMETER
@@ -202,14 +200,13 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
_height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf;
// Don't continue to grow wind velocity state variances if they are becoming too large or we are not using wind velocity states as this can make the covariance matrix badly conditioned
if (_control_status.flags.wind && (P(22,22) + P(23,23)) < sq(_params.initial_wind_uncertainty)) {
if (_control_status.flags.wind && P.trace<State::wind_vel.dof>(State::wind_vel.idx) < sq(_params.initial_wind_uncertainty)) {
wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.0f, 1.0f) * (1.0f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf));
} else {
wind_vel_nsd_scaled = 0.0f;
}
// assign IMU noise variances
// inputs to the system are 3 delta angles and 3 delta velocities
float gyro_noise = math::constrain(_params.gyro_noise, 0.0f, 1.0f);
@@ -219,7 +216,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
Vector3f d_vel_var;
for (int i = 0; i <= 2; i++) {
for (unsigned i = 0; i < 3; i++) {
if (_fault_status.flags.bad_acc_vertical || imu_delayed.delta_vel_clipping[i]) {
// Increase accelerometer process noise if bad accel data is detected
d_vel_var(i) = sq(imu_delayed.delta_vel_dt * BADACC_BIAS_PNOISE);
@@ -238,54 +235,87 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
imu_delayed.delta_ang, imu_delayed.delta_ang_dt, d_ang_var,
&nextP);
// compute noise variance for stationary processes
Vector24f process_noise;
// 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
// earth frame magnetic field states
process_noise.slice<3, 1>(16, 0) = sq(mag_I_sig);
// body frame magnetic field states
process_noise.slice<3, 1>(19, 0) = sq(mag_B_sig);
// wind velocity states
process_noise.slice<2, 1>(22, 0) = sq(wind_vel_nsd_scaled) * dt;
// add process noise that is not from the IMU
for (unsigned i = 16; i <= 23; i++) {
nextP(i, i) += process_noise(i);
}
// gyro bias: add process noise, or restore previous gyro bias var if state inhibited
const float gyro_bias_sig = dt * math::constrain(_params.gyro_bias_p_noise, 0.f, 1.f);
const float gyro_bias_process_noise = sq(gyro_bias_sig);
for (unsigned i = 10; i <= 12; i++) {
const int axis_index = i - 10;
for (unsigned index = 0; index < State::gyro_bias.dof; index++) {
const unsigned i = State::gyro_bias.idx + index;
if (!_gyro_bias_inhibit[axis_index]) {
if (!_gyro_bias_inhibit[index]) {
nextP(i, i) += gyro_bias_process_noise;
} else {
nextP.uncorrelateCovarianceSetVariance<1>(i, _prev_gyro_bias_var(axis_index));
nextP.uncorrelateCovarianceSetVariance<1>(i, _prev_gyro_bias_var(index));
}
}
// accel bias: add process noise, or restore previous accel bias var if state inhibited
const float accel_bias_sig = dt * math::constrain(_params.accel_bias_p_noise, 0.f, 1.f);
const float accel_bias_process_noise = sq(accel_bias_sig);
for (int i = 13; i <= 15; i++) {
const int axis_index = i - 13;
for (unsigned index = 0; index < State::accel_bias.dof; index++) {
const unsigned i = State::accel_bias.idx + index;
if (!_accel_bias_inhibit[axis_index]) {
if (!_accel_bias_inhibit[index]) {
nextP(i, i) += accel_bias_process_noise;
} else {
nextP.uncorrelateCovarianceSetVariance<1>(i, _prev_accel_bias_var(axis_index));
nextP.uncorrelateCovarianceSetVariance<1>(i, _prev_accel_bias_var(index));
}
}
if (_control_status.flags.mag) {
const float mag_I_process_noise = sq(mag_I_sig);
for (unsigned index = 0; index < State::mag_I.dof; index++) {
unsigned i = State::mag_I.idx + index;
nextP(i, i) += mag_I_process_noise;
}
const float mag_B_process_noise = sq(mag_B_sig);
for (unsigned index = 0; index < State::mag_B.dof; index++) {
unsigned i = State::mag_B.idx + index;
nextP(i, i) += mag_B_process_noise;
}
} else {
// keep previous covariance
for (unsigned i = 0; i < State::mag_I.dof; i++) {
unsigned row = State::mag_I.idx + i;
for (unsigned col = 0; col < _k_num_states; col++) {
nextP(row, col) = nextP(col, row) = P(row, col);
}
}
for (unsigned i = 0; i < State::mag_B.dof; i++) {
unsigned row = State::mag_B.idx + i;
for (unsigned col = 0; col < _k_num_states; col++) {
nextP(row, col) = nextP(col, row) = P(row, col);
}
}
}
if (_control_status.flags.wind) {
const float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt;
;
for (unsigned index = 0; index < State::wind_vel.dof; index++) {
unsigned i = State::wind_vel.idx + index;
nextP(i, i) += wind_vel_process_noise;
}
} else {
// keep previous covariance
for (unsigned i = 0; i < State::wind_vel.dof; i++) {
unsigned row = State::wind_vel.idx + i;
for (unsigned col = 0 ; col < _k_num_states; col++) {
nextP(row, col) = nextP(col, row) = P(row, col);
}
}
}
// covariance matrix is symmetrical, so copy upper half to lower half
for (unsigned row = 0; row <= 15; row++) {
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 0 ; column < row; column++) {
P(row, column) = P(column, row) = nextP(column, row);
}
@@ -293,30 +323,9 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
P(row, row) = nextP(row, row);
}
if (_control_status.flags.mag) {
for (unsigned row = 16; row <= 21; row++) {
for (unsigned column = 0 ; column < row; column++) {
P(row, column) = P(column, row) = nextP(column, row);
}
P(row, row) = nextP(row, row);
}
}
if (_control_status.flags.wind) {
for (unsigned row = 22; row <= 23; row++) {
for (unsigned column = 0 ; column < row; column++) {
P(row, column) = P(column, row) = nextP(column, row);
}
P(row, row) = nextP(row, row);
}
}
// fix gross errors in the covariance matrix and ensure rows and
// columns for un-used states are zero
fixCovarianceErrors(false);
}
void Ekf::fixCovarianceErrors(bool force_symmetry)
@@ -336,29 +345,32 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
P_lim[6] = 1.0f; // body mag field max var
P_lim[7] = 1e6f; // wind max var
for (int i = 0; i <= 3; i++) {
for (unsigned i = State::quat_nominal.idx; i < (State::quat_nominal.idx + State::quat_nominal.dof); i++) {
// quaternion states
P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[0]);
}
for (int i = 4; i <= 6; i++) {
for (unsigned i = State::vel.idx; i < (State::vel.idx + State::vel.dof); i++) {
// NED velocity states
P(i, i) = math::constrain(P(i, i), 1e-6f, P_lim[1]);
}
for (int i = 7; i <= 9; i++) {
for (unsigned i = State::pos.idx; i < (State::pos.idx + State::pos.dof); i++) {
// NED position states
P(i, i) = math::constrain(P(i, i), 1e-6f, P_lim[2]);
}
for (int i = 10; i <= 12; i++) {
for (unsigned i = State::gyro_bias.idx; i < (State::gyro_bias.idx + State::gyro_bias.dof); i++) {
// gyro bias states
P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[3]);
}
// force symmetry on the quaternion, velocity and position state covariances
if (force_symmetry) {
P.makeRowColSymmetric<13>(0);
P.makeRowColSymmetric<State::quat_nominal.dof>(State::quat_nominal.idx);
P.makeRowColSymmetric<State::vel.dof>(State::vel.idx);
P.makeRowColSymmetric<State::pos.dof>(State::pos.idx);
P.makeRowColSymmetric<State::gyro_bias.dof>(State::gyro_bias.idx); //TODO: needed?
}
// the following states are optional and are deactivated when not required
@@ -371,8 +383,10 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
float maxStateVar = minSafeStateVar;
bool resetRequired = false;
for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) {
if (_accel_bias_inhibit[stateIndex - 13]) {
for (unsigned axis = 0; axis < State::accel_bias.dof; axis++) {
const unsigned stateIndex = State::accel_bias.idx + axis;
if (_accel_bias_inhibit[axis]) {
// Skip the check for the inhibited axis
continue;
}
@@ -391,8 +405,10 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
const float minStateVarTarget = 5E-8f / sq(_dt_ekf_avg);
float minAllowedStateVar = fmaxf(0.01f * maxStateVar, minStateVarTarget);
for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) {
if (_accel_bias_inhibit[stateIndex - 13]) {
for (unsigned axis = 0; axis < State::accel_bias.dof; axis++) {
const unsigned stateIndex = State::accel_bias.idx + axis;
if (_accel_bias_inhibit[axis]) {
// Skip the check for the inhibited axis
continue;
}
@@ -402,7 +418,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
// If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero
if (resetRequired) {
P.uncorrelateCovariance<3>(13);
P.uncorrelateCovariance<State::accel_bias.dof>(State::accel_bias.idx);
}
// Run additional checks to see if the delta velocity bias has hit limits in a direction that is clearly wrong
@@ -461,7 +477,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
// the covariance matrix but preserve the variances (diagonals) to allow bias learning to continue
if (isTimedOut(_time_acc_bias_check, (uint64_t)7e6)) {
P.uncorrelateCovariance<3>(13);
P.uncorrelateCovariance<State::accel_bias.dof>(State::accel_bias.idx);
_time_acc_bias_check = _time_delayed_us;
_fault_status.flags.bad_acc_bias = false;
@@ -470,7 +486,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
} else if (force_symmetry) {
// ensure the covariance values are symmetrical
P.makeRowColSymmetric<3>(13);
P.makeRowColSymmetric<State::accel_bias.dof>(State::accel_bias.idx);
}
}
@@ -482,35 +498,35 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
} else {
// constrain variances
for (int i = 16; i <= 18; i++) {
for (unsigned i = State::mag_I.idx; i < (State::mag_I.idx + State::mag_I.dof); i++) {
P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[5]);
}
for (int i = 19; i <= 21; i++) {
for (unsigned i = State::mag_B.idx; i < (State::mag_B.idx + State::mag_B.dof); i++) {
P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[6]);
}
// force symmetry
if (force_symmetry) {
P.makeRowColSymmetric<3>(16);
P.makeRowColSymmetric<3>(19);
P.makeRowColSymmetric<State::mag_I.dof>(State::mag_I.idx);
P.makeRowColSymmetric<State::mag_B.dof>(State::mag_B.idx);
}
}
// wind velocity states
if (!_control_status.flags.wind) {
P.uncorrelateCovarianceSetVariance<2>(22, 0.0f);
P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, 0.0f);
} else {
// constrain variances
for (int i = 22; i <= 23; i++) {
for (unsigned i = State::wind_vel.idx; i < (State::wind_vel.idx + State::wind_vel.dof); i++) {
P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[7]);
}
// force symmetry
if (force_symmetry) {
P.makeRowColSymmetric<2>(22);
P.makeRowColSymmetric<State::wind_vel.dof>(State::wind_vel.idx);
}
}
}
@@ -543,13 +559,14 @@ void Ekf::resetQuatCov(const float yaw_noise)
}
// clear existing quaternion covariance
P.uncorrelateCovarianceSetVariance<2>(0, 0.0f);
P.uncorrelateCovarianceSetVariance<2>(2, 0.0f);
// Optimization: avoid the creation of a <4> function
P.uncorrelateCovarianceSetVariance<2>(State::quat_nominal.idx, 0.0f);
P.uncorrelateCovarianceSetVariance<2>(State::quat_nominal.idx + 2, 0.0f);
matrix::SquareMatrix<float, 4> q_cov;
matrix::SquareMatrix<float, State::quat_nominal.dof> q_cov;
sym::RotVarNedToLowerTriangularQuatCov(getStateAtFusionHorizonAsVector(), rot_var_ned, &q_cov);
q_cov.copyLowerToUpperTriangle();
P.slice<4, 4>(0, 0) = q_cov;
P.slice<State::quat_nominal.dof, State::quat_nominal.dof>(State::quat_nominal.idx, State::quat_nominal.idx) = q_cov;
}
void Ekf::resetMagCov()
@@ -560,8 +577,8 @@ void Ekf::resetMagCov()
_mag_decl_cov_reset = false;
}
P.uncorrelateCovarianceSetVariance<3>(16, sq(_params.mag_noise));
P.uncorrelateCovarianceSetVariance<3>(19, sq(_params.mag_noise));
P.uncorrelateCovarianceSetVariance<State::mag_I.dof>(State::mag_I.idx, sq(_params.mag_noise));
P.uncorrelateCovarianceSetVariance<State::mag_B.dof>(State::mag_B.idx, sq(_params.mag_noise));
saveMagCovData();
#else
@@ -575,5 +592,5 @@ void Ekf::resetGyroBiasZCov()
{
const float init_gyro_bias_var = sq(_params.switch_on_gyro_bias);
P.uncorrelateCovarianceSetVariance<1>(12, init_gyro_bias_var);
P.uncorrelateCovarianceSetVariance<1>(State::gyro_bias.idx + 2, init_gyro_bias_var);
}