From 028733e1c7eb98ecc3bf43a67d99e59ff76903f0 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 3 Oct 2023 20:22:33 -0400 Subject: [PATCH] ekf2: add kconfig to disable wind estimation (off by default) --- src/modules/ekf2/EKF/common.h | 5 +++- src/modules/ekf2/EKF/covariance.cpp | 38 ++++++++++++++--------------- src/modules/ekf2/EKF/ekf.cpp | 4 +++ src/modules/ekf2/EKF/ekf.h | 7 ++++-- src/modules/ekf2/EKF/ekf_helper.cpp | 2 ++ src/modules/ekf2/EKF2.cpp | 12 +++++++++ src/modules/ekf2/EKF2.hpp | 18 ++++++++++---- src/modules/ekf2/Kconfig | 11 +++++++++ 8 files changed, 69 insertions(+), 28 deletions(-) diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 1c53a8a961..0a6e23f074 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -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) diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index f6363e392a..bf359a6cea 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -88,8 +88,10 @@ void Ekf::initialiseCovariance() resetMagCov(); +#if defined(CONFIG_EKF2_WIND) // wind P.uncorrelateCovarianceSetVariance(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.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.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.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.idx); } +#endif // CONFIG_EKF2_WIND } } diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index bf96423bb4..f0f7f98088 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -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; } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index f69b1ece3c..69154c30ab 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -306,9 +306,11 @@ public: // get the state vector at the delayed time horizon const matrix::Vector &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(); } +#endif // CONFIG_EKF2_WIND template matrix::VectorgetStateVariance() const { return P.slice(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(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 92bee563f0..1f927f6675 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -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.idx, _params.initial_wind_uncertainty); } +#endif // CONFIG_EKF2_WIND diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index e2aa9507b2..96164f4a52 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -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 ×tamp) } } +#if defined(CONFIG_EKF2_WIND) void EKF2::PublishWindEstimate(const hrt_abstime ×tamp) { if (_ekf.get_wind_status()) { @@ -1981,6 +1992,7 @@ void EKF2::PublishWindEstimate(const hrt_abstime ×tamp) _wind_pub.publish(wind); } } +#endif // CONFIG_EKF2_WIND #if defined(CONFIG_EKF2_OPTICAL_FLOW) void EKF2::PublishOpticalFlowVel(const hrt_abstime ×tamp) diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 8eea2e9dec..ab95aa1180 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -86,7 +86,6 @@ #include #include #include -#include #include #if defined(CONFIG_EKF2_AIRSPEED) @@ -115,6 +114,10 @@ # include #endif // CONFIG_EKF2_RANGE_FINDER +#if defined(CONFIG_EKF2_WIND) +# include +#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 ×tamp); void PublishLocalPosition(const hrt_abstime ×tamp); void PublishOdometry(const hrt_abstime ×tamp, const imuSample &imu_sample); - void PublishOdometryAligned(const hrt_abstime ×tamp, const vehicle_odometry_s &ev_odom); #if defined(CONFIG_EKF2_OPTICAL_FLOW) void PublishOpticalFlowVel(const hrt_abstime ×tamp); #endif // CONFIG_EKF2_OPTICAL_FLOW @@ -204,7 +206,9 @@ private: void PublishStates(const hrt_abstime ×tamp); void PublishStatus(const hrt_abstime ×tamp); void PublishStatusFlags(const hrt_abstime ×tamp); +#if defined(CONFIG_EKF2_WIND) void PublishWindEstimate(const hrt_abstime ×tamp); +#endif // CONFIG_EKF2_WIND void PublishYawEstimatorStatus(const hrt_abstime ×tamp); #if defined(CONFIG_EKF2_AIRSPEED) @@ -497,8 +501,10 @@ private: uORB::PublicationMulti _local_position_pub; uORB::PublicationMulti _global_position_pub; uORB::PublicationMulti _odometry_pub; - uORB::PublicationMulti _wind_pub; +#if defined(CONFIG_EKF2_WIND) + uORB::PublicationMulti _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) _param_ekf2_acc_b_noise,///< process noise for IMU accelerometer bias prediction (m/sec**3) - (ParamExtFloat) - _param_ekf2_wind_nsd, ///< process noise spectral density for wind velocity prediction (m/sec**2/sqrt(Hz)) + +#if defined(CONFIG_EKF2_WIND) + (ParamExtFloat) _param_ekf2_wind_nsd, +#endif // CONFIG_EKF2_WIND (ParamExtFloat) _param_ekf2_gps_v_noise, ///< minimum allowed observation noise for gps velocity fusion (m/sec) diff --git a/src/modules/ekf2/Kconfig b/src/modules/ekf2/Kconfig index a89b68b838..fc47a4f3d7 100644 --- a/src/modules/ekf2/Kconfig +++ b/src/modules/ekf2/Kconfig @@ -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