fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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-03 20:22:33 -04:00
parent c41de22a05
commit 028733e1c7
8 changed files with 69 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/modules/ekf2/EKF/common.h
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)

38
src/modules/ekf2/EKF/covariance.cpp
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
}
}

4
src/modules/ekf2/EKF/ekf.cpp
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;
}

7
src/modules/ekf2/EKF/ekf.h
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();

2
src/modules/ekf2/EKF/ekf_helper.cpp
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

12
src/modules/ekf2/EKF2.cpp
View File

@ -56,7 +56,9 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_local_position_pub(multi_mode ? ORB_ID(estimator_local_position) : ORB_ID(vehicle_local_position)),
_global_position_pub(multi_mode ? ORB_ID(estimator_global_position) : ORB_ID(vehicle_global_position)),
_odometry_pub(multi_mode ? ORB_ID(estimator_odometry) : ORB_ID(vehicle_odometry)),
#if defined(CONFIG_EKF2_WIND)
_wind_pub(multi_mode ? ORB_ID(estimator_wind) : ORB_ID(wind)),
#endif // CONFIG_EKF2_WIND
_params(_ekf.getParamHandle()),
_param_ekf2_predict_us(_params->filter_update_interval_us),
_param_ekf2_imu_ctrl(_params->imu_ctrl),
@ -68,7 +70,9 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_param_ekf2_acc_noise(_params->accel_noise),
_param_ekf2_gyr_b_noise(_params->gyro_bias_p_noise),
_param_ekf2_acc_b_noise(_params->accel_bias_p_noise),
#if defined(CONFIG_EKF2_WIND)
_param_ekf2_wind_nsd(_params->wind_vel_nsd),
#endif // CONFIG_EKF2_WIND
_param_ekf2_gps_v_noise(_params->gps_vel_noise),
_param_ekf2_gps_p_noise(_params->gps_pos_noise),
_param_ekf2_noaid_noise(_params->pos_noaid_noise),
@ -340,7 +344,10 @@ bool EKF2::multi_init(int imu, int mag)
_local_position_pub.advertise();
_global_position_pub.advertise();
_odometry_pub.advertise();
#if defined(CONFIG_EKF2_WIND)
_wind_pub.advertise();
#endif // CONFIG_EKF2_WIND
bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu);
@ -732,7 +739,10 @@ void EKF2::Run()
PublishLocalPosition(now);
PublishOdometry(now, imu_sample_new);
PublishGlobalPosition(now);
#if defined(CONFIG_EKF2_WIND)
PublishWindEstimate(now);
#endif // CONFIG_EKF2_WIND
// publish status/logging messages
#if defined(CONFIG_EKF2_BAROMETER)
@ -1953,6 +1963,7 @@ void EKF2::PublishYawEstimatorStatus(const hrt_abstime &timestamp)
}
}
#if defined(CONFIG_EKF2_WIND)
void EKF2::PublishWindEstimate(const hrt_abstime &timestamp)
{
if (_ekf.get_wind_status()) {
@ -1981,6 +1992,7 @@ void EKF2::PublishWindEstimate(const hrt_abstime &timestamp)
_wind_pub.publish(wind);
}
}
#endif // CONFIG_EKF2_WIND
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
void EKF2::PublishOpticalFlowVel(const hrt_abstime &timestamp)

18
src/modules/ekf2/EKF2.hpp
View File

@ -86,7 +86,6 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_odometry.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/wind.h>
#include <uORB/topics/yaw_estimator_status.h>
#if defined(CONFIG_EKF2_AIRSPEED)
@ -115,6 +114,10 @@
# include <uORB/topics/distance_sensor.h>
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_WIND)
# include <uORB/topics/wind.h>
#endif // CONFIG_EKF2_WIND
extern pthread_mutex_t ekf2_module_mutex;
class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
@ -196,7 +199,6 @@ private:
void PublishInnovationVariances(const hrt_abstime &timestamp);
void PublishLocalPosition(const hrt_abstime &timestamp);
void PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu_sample);
void PublishOdometryAligned(const hrt_abstime &timestamp, const vehicle_odometry_s &ev_odom);
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
void PublishOpticalFlowVel(const hrt_abstime &timestamp);
#endif // CONFIG_EKF2_OPTICAL_FLOW
@ -204,7 +206,9 @@ private:
void PublishStates(const hrt_abstime &timestamp);
void PublishStatus(const hrt_abstime &timestamp);
void PublishStatusFlags(const hrt_abstime &timestamp);
#if defined(CONFIG_EKF2_WIND)
void PublishWindEstimate(const hrt_abstime &timestamp);
#endif // CONFIG_EKF2_WIND
void PublishYawEstimatorStatus(const hrt_abstime &timestamp);
#if defined(CONFIG_EKF2_AIRSPEED)
@ -497,8 +501,10 @@ private:
uORB::PublicationMulti<vehicle_local_position_s> _local_position_pub;
uORB::PublicationMulti<vehicle_global_position_s> _global_position_pub;
uORB::PublicationMulti<vehicle_odometry_s> _odometry_pub;
uORB::PublicationMulti<wind_s> _wind_pub;
#if defined(CONFIG_EKF2_WIND)
uORB::PublicationMulti<wind_s> _wind_pub;
#endif // CONFIG_EKF2_WIND
PreFlightChecker _preflt_checker;
@ -528,8 +534,10 @@ private:
_param_ekf2_gyr_b_noise, ///< process noise for IMU rate gyro bias prediction (rad/sec**2)
(ParamExtFloat<px4::params::EKF2_ACC_B_NOISE>)
_param_ekf2_acc_b_noise,///< process noise for IMU accelerometer bias prediction (m/sec**3)
(ParamExtFloat<px4::params::EKF2_WIND_NSD>)
_param_ekf2_wind_nsd, ///< process noise spectral density for wind velocity prediction (m/sec**2/sqrt(Hz))
#if defined(CONFIG_EKF2_WIND)
(ParamExtFloat<px4::params::EKF2_WIND_NSD>) _param_ekf2_wind_nsd,
#endif // CONFIG_EKF2_WIND
(ParamExtFloat<px4::params::EKF2_GPS_V_NOISE>)
_param_ekf2_gps_v_noise, ///< minimum allowed observation noise for gps velocity fusion (m/sec)

11
src/modules/ekf2/Kconfig
View File

@ -18,6 +18,7 @@ depends on MODULES_EKF2
bool "airspeed fusion support"
default y
depends on EKF2_SIDESLIP
depends on EKF2_WIND
---help---
EKF2 airspeed fusion support.
@ -40,6 +41,7 @@ depends on MODULES_EKF2
bool "barometer compensation support"
default y
depends on EKF2_BAROMETER
depends on EKF2_WIND
---help---
EKF2 pressure compensation support.
@ -47,6 +49,7 @@ menuconfig EKF2_DRAG_FUSION
depends on MODULES_EKF2
bool "drag fusion support"
default y
depends on EKF2_WIND
---help---
EKF2 drag fusion support.
@ -91,6 +94,7 @@ menuconfig EKF2_SIDESLIP
depends on MODULES_EKF2
bool "sideslip fusion support"
default y
depends on EKF2_WIND
---help---
EKF2 sideslip fusion support.
@ -102,6 +106,13 @@ depends on MODULES_EKF2
---help---
EKF2 terrain estimator support.
menuconfig EKF2_WIND
depends on MODULES_EKF2
bool "wind estimation support"
default y
---help---
EKF2 wind estimation support.
menuconfig USER_EKF2
bool "ekf2 running as userspace module"
default n