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

ekf2: cleanup more optional mag (CONFIG_EKF2_MAGNETOMETER)

Browse Source
This commit is contained in:
Daniel Agar
2023-10-09 10:18:02 -04:00
parent 5f87f3a046
commit 28d58a947f
7 changed files with 71 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/covariance.cpp
+29 -31
View File
@@ -177,24 +177,6 @@ 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.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
}
// 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.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
}
// 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);
@@ -255,18 +237,31 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
}
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;
#if defined(CONFIG_EKF2_MAGNETOMETER)
// 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
if (P.trace<State::mag_I.dof>(State::mag_I.idx) < 0.1f) {
float mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.f, 1.f);
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;
}
// Don't continue to grow the body 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
if (P.trace<State::mag_B.dof>(State::mag_B.idx) < 0.1f) {
float mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.f, 1.f);
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;
}
}
#endif // CONFIG_EKF2_MAGNETOMETER
} else {
// keep previous covariance
for (unsigned i = 0; i < State::mag_I.dof; i++) {
@@ -335,8 +330,6 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
const float vel_var_max = 1e6f;
const float pos_var_max = 1e6f;
const float gyro_bias_var_max = 1.0f;
const float mag_I_var_max = 1.0f;
const float mag_B_var_max = 1.0f;
constrainStateVar(State::quat_nominal, 0.f, quat_var_max);
constrainStateVar(State::vel, 1e-6f, vel_var_max);
@@ -471,17 +464,22 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
// magnetic field states
if (!_control_status.flags.mag) {
P.uncorrelateCovarianceSetVariance<3>(16, 0.0f);
P.uncorrelateCovarianceSetVariance<3>(19, 0.0f);
P.uncorrelateCovarianceSetVariance<State::mag_I.dof>(State::mag_I.idx, 0.0f);
P.uncorrelateCovarianceSetVariance<State::mag_B.dof>(State::mag_B.idx, 0.0f);
} else {
#if defined(CONFIG_EKF2_MAGNETOMETER)
const float mag_I_var_max = 1.f;
constrainStateVar(State::mag_I, 0.f, mag_I_var_max);
const float mag_B_var_max = 1.f;
constrainStateVar(State::mag_B, 0.f, mag_B_var_max);
if (force_symmetry) {
P.makeRowColSymmetric<State::mag_I.dof>(State::mag_I.idx);
P.makeRowColSymmetric<State::mag_B.dof>(State::mag_B.idx);
}
#endif // CONFIG_EKF2_MAGNETOMETER
}
// wind velocity states