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

ekf2: symforce derivation allow optionally disabling mag and wind states

Browse Source
This commit is contained in:
Daniel Agar
2023-10-10 17:31:11 -04:00
committed by GitHub
parent b5f3d089c4
commit 5352a64042
15 changed files with 162 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/covariance.cpp
+13 -14
View File
@@ -42,8 +42,8 @@
*/
#include "ekf.h"
#include "python/ekf_derivation/generated/predict_covariance.h"
#include "python/ekf_derivation/generated/rot_var_ned_to_lower_triangular_quat_cov.h"
#include <ekf_derivation/generated/predict_covariance.h>
#include <ekf_derivation/generated/rot_var_ned_to_lower_triangular_quat_cov.h>
#include <math.h>
#include <mathlib/mathlib.h>
@@ -86,7 +86,9 @@ void Ekf::initialiseCovariance()
P.uncorrelateCovarianceSetVariance<State::accel_bias.dof>(State::accel_bias.idx, accel_bias_var);
_prev_accel_bias_var.setAll(accel_bias_var);
#if defined(CONFIG_EKF2_MAGNETOMETER)
resetMagCov();
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_WIND)
// wind
@@ -236,8 +238,8 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
}
}
if (_control_status.flags.mag) {
#if defined(CONFIG_EKF2_MAGNETOMETER)
if (_control_status.flags.mag) {
// 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) {
@@ -261,7 +263,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
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++) {
@@ -278,6 +280,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
}
}
}
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_WIND)
if (_control_status.flags.wind) {
@@ -462,13 +465,13 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
}
#if defined(CONFIG_EKF2_MAGNETOMETER)
// magnetic field states
if (!_control_status.flags.mag) {
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);
@@ -479,23 +482,23 @@ void Ekf::fixCovarianceErrors(bool 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
}
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_WIND)
// wind velocity states
if (!_control_status.flags.wind) {
P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, 0.0f);
} else {
#if defined(CONFIG_EKF2_WIND)
const float wind_vel_var_max = 1e6f;
constrainStateVar(State::wind_vel, 0.f, wind_vel_var_max);
if (force_symmetry) {
P.makeRowColSymmetric<State::wind_vel.dof>(State::wind_vel.idx);
}
#endif // CONFIG_EKF2_WIND
}
#endif // CONFIG_EKF2_WIND
}
void Ekf::constrainStateVar(const IdxDof &state, float min, float max)
@@ -543,9 +546,9 @@ void Ekf::resetQuatCov(const Vector3f &rot_var_ned)
resetStateCovariance<State::quat_nominal>(q_cov);
}
#if defined(CONFIG_EKF2_MAGNETOMETER)
void Ekf::resetMagCov()
{
#if defined(CONFIG_EKF2_MAGNETOMETER)
if (_mag_decl_cov_reset) {
ECL_INFO("reset mag covariance");
_mag_decl_cov_reset = false;
@@ -555,12 +558,8 @@ void Ekf::resetMagCov()
P.uncorrelateCovarianceSetVariance<State::mag_B.dof>(State::mag_B.idx, sq(_params.mag_noise));
saveMagCovData();
#else
P.uncorrelateCovarianceSetVariance<State::mag_I.dof>(State::mag_I.idx, 0.f);
P.uncorrelateCovarianceSetVariance<State::mag_B.dof>(State::mag_B.idx, 0.f);
#endif
}
#endif // CONFIG_EKF2_MAGNETOMETER
void Ekf::resetGyroBiasZCov()
{