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

ekf2: add kconfig to disable wind estimation (off by default)

Browse Source
This commit is contained in:
Daniel Agar
2023-10-09 09:22:29 -04:00
parent c41de22a05
commit 028733e1c7
8 changed files with 69 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/common.h
+4 -1
View File
@@ -297,14 +297,17 @@ struct parameters {
float accel_bias_p_noise{1.0e-2f}; ///< process noise for IMU accelerometer bias prediction (m/sec**3)
float mage_p_noise{1.0e-3f}; ///< process noise for earth magnetic field prediction (Gauss/sec)
float magb_p_noise{1.0e-4f}; ///< process noise for body magnetic field prediction (Gauss/sec)
#if defined(CONFIG_EKF2_WIND)
const float initial_wind_uncertainty{1.0f}; ///< 1-sigma initial uncertainty in wind velocity (m/sec)
float wind_vel_nsd{1.0e-2f}; ///< process noise spectral density for wind velocity prediction (m/sec**2/sqrt(Hz))
const float wind_vel_nsd_scaler{0.5f}; ///< scaling of wind process noise with vertical velocity
#endif // CONFIG_EKF2_WIND
// initialization errors
float switch_on_gyro_bias{0.1f}; ///< 1-sigma gyro bias uncertainty at switch on (rad/sec)
float switch_on_accel_bias{0.2f}; ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2)
float initial_tilt_err{0.1f}; ///< 1-sigma tilt error after initial alignment using gravity vector (rad)
const float initial_wind_uncertainty{1.0f}; ///< 1-sigma initial uncertainty in wind velocity (m/sec)
// position and velocity fusion
float gps_vel_noise{0.5f}; ///< minimum allowed observation noise for gps velocity fusion (m/sec)
src/modules/ekf2/EKF/covariance.cpp
+18 -20
View File
@@ -88,8 +88,10 @@ void Ekf::initialiseCovariance()
resetMagCov();
#if defined(CONFIG_EKF2_WIND)
// wind
P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, sq(_params.initial_wind_uncertainty));
#endif // CONFIG_EKF2_WIND
}
void Ekf::predictCovariance(const imuSample &imu_delayed)
@@ -193,20 +195,6 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
#endif // CONFIG_EKF2_MAGNETOMETER
}
float wind_vel_nsd_scaled;
// Calculate low pass filtered height rate
float alpha_height_rate_lpf = 0.1f * dt; // 10 seconds time constant
_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.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);
@@ -296,23 +284,31 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
}
}
#if defined(CONFIG_EKF2_WIND)
if (_control_status.flags.wind) {
const float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt;
// 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 (P.trace<State::wind_vel.dof>(State::wind_vel.idx) < sq(_params.initial_wind_uncertainty)) {
for (unsigned index = 0; index < State::wind_vel.dof; index++) {
unsigned i = State::wind_vel.idx + index;
nextP(i, i) += wind_vel_process_noise;
float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f, 1.f) * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf));
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 < State::size; col++) {
for (unsigned col = 0; col < State::size; col++) {
nextP(row, col) = nextP(col, row) = P(row, col);
}
}
}
#endif // CONFIG_EKF2_WIND
// covariance matrix is symmetrical, so copy upper half to lower half
for (unsigned row = 0; row < State::size; row++) {
@@ -341,7 +337,6 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
const float gyro_bias_var_max = 1.0f;
const float mag_I_var_max = 1.0f;
const float mag_B_var_max = 1.0f;
const float wind_vel_var_max = 1e6f;
constrainStateVar(State::quat_nominal, 0.f, quat_var_max);
constrainStateVar(State::vel, 1e-6f, vel_var_max);
@@ -494,11 +489,14 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
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
}
}
src/modules/ekf2/EKF/ekf.cpp
+4
View File
@@ -324,4 +324,8 @@ void Ekf::predictState(const imuSample &imu_delayed)
// Calculate filtered yaw rate to be used by the magnetometer fusion type selection logic
// Note fixed coefficients are used to save operations. The exact time constant is not important.
_yaw_rate_lpf_ef = 0.95f * _yaw_rate_lpf_ef + 0.05f * spin_del_ang_D / imu_delayed.delta_ang_dt;
// Calculate low pass filtered height rate
float alpha_height_rate_lpf = 0.1f * imu_delayed.delta_vel_dt; // 10 seconds time constant
_height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf;
}
src/modules/ekf2/EKF/ekf.h
+5 -2
View File
@@ -306,9 +306,11 @@ public:
// get the state vector at the delayed time horizon
const matrix::Vector<float, State::size> &getStateAtFusionHorizonAsVector() const { return _state.vector(); }
#if defined(CONFIG_EKF2_WIND)
// get the wind velocity in m/s
const Vector2f &getWindVelocity() const { return _state.wind_vel; };
Vector2f getWindVelocityVariance() const { return getStateVariance<State::wind_vel>(); }
#endif // CONFIG_EKF2_WIND
template <const IdxDof &S>
matrix::Vector<float, S.dof>getStateVariance() const { return P.slice<S.dof, S.dof>(S.idx, S.idx).diag(); } // calling getStateCovariance().diag() uses more flash space
@@ -616,6 +618,7 @@ private:
float _zgup_delta_ang_dt{0.f};
Vector2f _accel_lpf_NE{}; ///< Low pass filtered horizontal earth frame acceleration (m/sec**2)
float _height_rate_lpf{0.0f};
float _yaw_delta_ef{0.0f}; ///< Recent change in yaw angle measured about the earth frame D axis (rad)
float _yaw_rate_lpf_ef{0.0f}; ///< Filtered angular rate about earth frame D axis (rad/sec)
@@ -788,8 +791,6 @@ private:
uint64_t _time_good_vert_accel{0}; ///< last time a good vertical accel was detected (uSec)
uint16_t _clip_counter{0}; ///< counter that increments when clipping ad decrements when not
float _height_rate_lpf{0.0f};
// initialise filter states of both the delayed ekf and the real time complementary filter
bool initialiseFilter(void);
@@ -1168,9 +1169,11 @@ private:
void resetMagCov();
#if defined(CONFIG_EKF2_WIND)
// perform a reset of the wind states and related covariances
void resetWind();
void resetWindToZero();
#endif // CONFIG_EKF2_WIND
void resetGyroBiasZCov();
src/modules/ekf2/EKF/ekf_helper.cpp
+2
View File
@@ -1037,6 +1037,7 @@ void Ekf::resetGpsDriftCheckFilters()
_gps_filtered_horizontal_velocity_m_s = NAN;
}
#if defined(CONFIG_EKF2_WIND)
void Ekf::resetWind()
{
#if defined(CONFIG_EKF2_AIRSPEED)
@@ -1059,3 +1060,4 @@ void Ekf::resetWindToZero()
// start with a small initial uncertainty to improve the initial estimate
P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, _params.initial_wind_uncertainty);
}
#endif // CONFIG_EKF2_WIND