diff --git a/docs/en/advanced_config/tuning_the_ecl_ekf.md b/docs/en/advanced_config/tuning_the_ecl_ekf.md index bd41fea8ee..97ccc682c0 100644 --- a/docs/en/advanced_config/tuning_the_ecl_ekf.md +++ b/docs/en/advanced_config/tuning_the_ecl_ekf.md @@ -386,7 +386,6 @@ It is further configured using the `EKF2_RNG_A_` parameters: - [EKF2_RNG_A_VMAX](../advanced_config/parameter_reference.md#EKF2_RNG_A_VMAX): Maximum horizontal speed, above which range aid is disabled. - [EKF2_RNG_A_HMAX](../advanced_config/parameter_reference.md#EKF2_RNG_A_HMAX): Maximum height, above which range aid is disabled. -- [EKF2_RNG_A_IGATE](../advanced_config/parameter_reference.md#EKF2_RNG_A_IGATE): Range aid consistency checks "gate" (a measure of the error before range aid is disabled). #### Range height fusion diff --git a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp index bf0025faee..b7b45d5ced 100644 --- a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp @@ -160,7 +160,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) } else { if (starting_conditions_passing) { - if (_params.height_sensor_ref == static_cast(HeightSensor::BARO)) { + if (_params.ekf2_hgt_ref == static_cast(HeightSensor::BARO)) { ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME); _height_sensor_ref = HeightSensor::BARO; diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp index aa4822fc30..5e26b059c8 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp @@ -167,7 +167,7 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp } else { if (starting_conditions_passing) { // activate fusion, only reset if necessary - if (_params.height_sensor_ref == static_cast(HeightSensor::EV)) { + if (_params.ekf2_hgt_ref == static_cast(HeightSensor::EV)) { ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME); _information_events.flags.reset_hgt_to_ev = true; resetAltitudeTo(-measurement, measurement_var); diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp index 599472a42f..a863a5b3e6 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp @@ -97,7 +97,7 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) const bool altitude_initialisation_conditions_passing = common_conditions_passing && !PX4_ISFINITE(_local_origin_alt) - && _params.height_sensor_ref == static_cast(HeightSensor::GNSS) + && _params.ekf2_hgt_ref == static_cast(HeightSensor::GNSS) && isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL); if (_control_status.flags.gps_hgt) { @@ -131,7 +131,7 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) } else { if (starting_conditions_passing) { - if (_params.height_sensor_ref == static_cast(HeightSensor::GNSS)) { + if (_params.ekf2_hgt_ref == static_cast(HeightSensor::GNSS)) { ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME); _height_sensor_ref = HeightSensor::GNSS; diff --git a/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp index 1a19f7898a..c88f7f7542 100644 --- a/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp @@ -59,7 +59,7 @@ void Ekf::controlGravityFusion(const imuSample &imu) && (accel_lpf_norm_sq < sq(upper_accel_limit)); // fuse gravity observation if our overall acceleration isn't too big - _control_status.flags.gravity_vector = (_params.imu_ctrl & static_cast(ImuCtrl::GravityVector)) + _control_status.flags.gravity_vector = (_params.ekf2_imu_ctrl & static_cast(ImuCtrl::GravityVector)) && (accel_lpf_norm_good || _control_status.flags.vehicle_at_rest) && !isHorizontalAidingActive() && _control_status.flags.tilt_align; // Let fake position do the initial alignment (more robust before takeoff) diff --git a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp index 92df26d477..4913ab78b2 100644 --- a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp @@ -241,7 +241,7 @@ void Ekf::resetTerrainToFlow() ECL_INFO("reset hagl to flow"); // TODO: use the flow data - const float new_terrain = -_gpos.altitude() + _params.rng_gnd_clearance; + const float new_terrain = -_gpos.altitude() + _params.ekf2_min_rng; const float delta_terrain = new_terrain - _state.terrain; _state.terrain = new_terrain; P.uncorrelateCovarianceSetVariance(State::terrain.idx, 100.f); diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp index e6cd566a3f..35d56422f2 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp @@ -52,10 +52,10 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) _range_sensor.setDataReadiness(rng_data_ready); // update range sensor angle parameters in case they have changed - _range_sensor.setPitchOffset(_params.rng_sens_pitch); + _range_sensor.setPitchOffset(_params.ekf2_rng_pitch); _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt); - _range_sensor.setQualityHysteresis(_params.range_valid_quality_s); - _range_sensor.setMaxFogDistance(_params.rng_fog); + _range_sensor.setQualityHysteresis(_params.ekf2_rng_qlty_t); + _range_sensor.setMaxFogDistance(_params.ekf2_rng_fog); _range_sensor.runChecks(imu_sample.time_us, _R_to_earth); @@ -69,10 +69,10 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) const bool horizontal_motion = _control_status.flags.fixed_wing || (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f)); - const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom()); - const float var = sq(_params.range_noise) + dist_dependant_var; + const float dist_dependant_var = sq(_params.ekf2_rng_sfe * _range_sensor.getDistBottom()); + const float var = sq(_params.ekf2_rng_noise) + dist_dependant_var; - _rng_consistency_check.setGate(_params.range_kin_consistency_gate); + _rng_consistency_check.setGate(_params.ekf2_rng_k_gate); _rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(State::vel.idx + 2, State::vel.idx + 2), horizontal_motion, imu_sample.time_us); } @@ -84,7 +84,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) && _range_sensor.isRegularlySendingData() && _range_sensor.isDataReady()) { - _range_sensor.setRange(_params.rng_gnd_clearance); + _range_sensor.setRange(_params.ekf2_min_rng); _range_sensor.setValidity(true); // bypass the checks } } @@ -102,8 +102,8 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) updateRangeHagl(aid_src); const bool measurement_valid = PX4_ISFINITE(aid_src.observation) && PX4_ISFINITE(aid_src.observation_variance); - const bool continuing_conditions_passing = ((_params.rng_ctrl == static_cast(RngCtrl::ENABLED)) - || (_params.rng_ctrl == static_cast(RngCtrl::CONDITIONAL))) + const bool continuing_conditions_passing = ((_params.ekf2_rng_ctrl == static_cast(RngCtrl::ENABLED)) + || (_params.ekf2_rng_ctrl == static_cast(RngCtrl::CONDITIONAL))) && _control_status.flags.tilt_align && measurement_valid && _range_sensor.isDataHealthy() @@ -115,11 +115,11 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) const bool do_conditional_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt) - && (_params.rng_ctrl == static_cast(RngCtrl::CONDITIONAL)) + && (_params.ekf2_rng_ctrl == static_cast(RngCtrl::CONDITIONAL)) && isConditionalRangeAidSuitable(); const bool do_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt) - && (_params.rng_ctrl == static_cast(RngCtrl::ENABLED)); + && (_params.ekf2_rng_ctrl == static_cast(RngCtrl::ENABLED)); if (_control_status.flags.rng_hgt) { if (!(do_conditional_range_aid || do_range_aid)) { @@ -128,7 +128,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } } else { - if (_params.height_sensor_ref == static_cast(HeightSensor::RANGE)) { + if (_params.ekf2_hgt_ref == static_cast(HeightSensor::RANGE)) { if (do_conditional_range_aid) { // Range finder is used while hovering to stabilize the height estimate. Don't reset but use it as height reference. ECL_INFO("starting conditional %s height fusion", HGT_SRC_NAME); @@ -242,13 +242,13 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src) { - const float measurement = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance); + const float measurement = math::max(_range_sensor.getDistBottom(), _params.ekf2_min_rng); const float measurement_variance = getRngVar(); float innovation_variance; sym::ComputeHaglInnovVar(P, measurement_variance, &innovation_variance); - const float innov_gate = math::max(_params.range_innov_gate, 1.f); + const float innov_gate = math::max(_params.ekf2_rng_gate, 1.f); updateAidSourceStatus(aid_src, _range_sensor.getSampleAddress()->time_us, // sample timestamp measurement, // observation @@ -270,8 +270,8 @@ float Ekf::getRngVar() const { return fmaxf( P(State::pos.idx + 2, State::pos.idx + 2) - + sq(_params.range_noise) - + sq(_params.range_noise_scaler * _range_sensor.getRange()), + + sq(_params.ekf2_rng_noise) + + sq(_params.ekf2_rng_sfe * _range_sensor.getRange()), 0.f); } @@ -313,16 +313,16 @@ bool Ekf::isConditionalRangeAidSuitable() { // check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching // Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice - float range_hagl_max = _params.max_hagl_for_range_aid; - float max_vel_xy = _params.max_vel_for_range_aid; + float range_hagl_max = _params.ekf2_rng_a_hmax; + float max_vel_xy = _params.ekf2_rng_a_vmax; const float hagl_test_ratio = _aid_src_rng_hgt.test_ratio; bool is_hagl_stable = (hagl_test_ratio < 1.f); if (!_control_status.flags.rng_hgt) { - range_hagl_max = 0.7f * _params.max_hagl_for_range_aid; - max_vel_xy = 0.7f * _params.max_vel_for_range_aid; + range_hagl_max = 0.7f * _params.ekf2_rng_a_hmax; + max_vel_xy = 0.7f * _params.ekf2_rng_a_vmax; is_hagl_stable = (hagl_test_ratio < 0.01f); } diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 29d96eb5f3..04f7409bfd 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -107,9 +107,9 @@ enum GeoDeclinationMask : uint8_t { enum MagFuseType : uint8_t { // Integer definitions for ekf2_mag_type - AUTO = 0, ///< The selection of either heading or 3D magnetometer fusion will be automatic - HEADING = 1, ///< Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg - NONE = 5, ///< Do not use magnetometer under any circumstance. + AUTO = 0, ///< The selection of either heading or 3D magnetometer fusion will be automatic + HEADING = 1, ///< Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg + NONE = 5, ///< Do not use magnetometer under any circumstance. INIT = 6 ///< Use the mag for heading initialization only. }; #endif // CONFIG_EKF2_MAGNETOMETER @@ -266,46 +266,46 @@ struct systemFlagUpdate { struct parameters { - int32_t ekf2_predict_us{10000}; ///< filter update interval in microseconds + int32_t ekf2_predict_us{10000}; ///< filter update interval in microseconds - int32_t imu_ctrl{static_cast(ImuCtrl::GyroBias) | static_cast(ImuCtrl::AccelBias)}; + int32_t ekf2_imu_ctrl{static_cast(ImuCtrl::GyroBias) | static_cast(ImuCtrl::AccelBias)}; - float ekf2_vel_lim{100.f}; ///< velocity state limit (m/s) + float ekf2_vel_lim{100.f}; ///< velocity state limit (m/s) // measurement source control - int32_t height_sensor_ref{static_cast(HeightSensor::BARO)}; + int32_t ekf2_hgt_ref{static_cast(HeightSensor::BARO)}; int32_t position_sensor_ref{static_cast(PositionSensor::GNSS)}; - float ekf2_delay_max{110.f}; ///< maximum time delay of all the aiding sensors. Sets the size of the observation buffers. (mSec) + float ekf2_delay_max{110.f}; ///< maximum time delay of all the aiding sensors. Sets the size of the observation buffers. (mSec) // input noise - float ekf2_gyr_noise{1.5e-2f}; ///< IMU angular rate noise used for covariance prediction (rad/sec) - float ekf2_acc_noise{3.5e-1f}; ///< IMU acceleration noise use for covariance prediction (m/sec**2) + float ekf2_gyr_noise{1.5e-2f}; ///< IMU angular rate noise used for covariance prediction (rad/sec) + float ekf2_acc_noise{3.5e-1f}; ///< IMU acceleration noise use for covariance prediction (m/sec**2) // process noise - float ekf2_gyr_b_noise{1.0e-3f}; ///< process noise for IMU rate gyro bias prediction (rad/sec**2) - float ekf2_acc_b_noise{1.0e-2f}; ///< process noise for IMU accelerometer bias prediction (m/sec**3) + float ekf2_gyr_b_noise{1.0e-3f}; ///< process noise for IMU rate gyro bias prediction (rad/sec**2) + float ekf2_acc_b_noise{1.0e-2f}; ///< process noise for IMU accelerometer bias prediction (m/sec**3) #if defined(CONFIG_EKF2_WIND) - const float initial_wind_uncertainty {1.0f}; ///< 1-sigma initial uncertainty in wind velocity (m/sec) - float ekf2_wind_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 + const float initial_wind_uncertainty {1.0f}; ///< 1-sigma initial uncertainty in wind velocity (m/sec) + float ekf2_wind_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 ekf2_gbias_init{0.1f}; ///< 1-sigma gyro bias uncertainty at switch on (rad/sec) - float ekf2_abias_init{0.2f}; ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2) + float ekf2_gbias_init{0.1f}; ///< 1-sigma gyro bias uncertainty at switch on (rad/sec) + float ekf2_abias_init{0.2f}; ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2) float ekf2_angerr_init{0.1f}; ///< 1-sigma tilt error after initial alignment using gravity vector (rad) #if defined(CONFIG_EKF2_BAROMETER) int32_t ekf2_baro_ctrl {1}; - float ekf2_baro_delay{0.0f}; ///< barometer height measurement delay relative to the IMU (mSec) - float ekf2_baro_noise{2.0f}; ///< observation noise for barometric height fusion (m) + float ekf2_baro_delay{0.0f}; ///< barometer height measurement delay relative to the IMU (mSec) + float ekf2_baro_noise{2.0f}; ///< observation noise for barometric height fusion (m) float baro_bias_nsd{0.13f}; ///< process noise for barometric height bias estimation (m/s/sqrt(Hz)) - float ekf2_baro_gate{5.0f}; ///< barometric and GPS height innovation consistency gate size (STD) + float ekf2_baro_gate{5.0f}; ///< barometric and GPS height innovation consistency gate size (STD) - float ekf2_gnd_eff_dz{5.0f}; ///< Size of deadzone applied to negative baro innovations when ground effect compensation is active (m) - float ekf2_gnd_max_hgt{0.5f}; ///< Height above ground at which baro ground effect becomes insignificant (m) + float ekf2_gnd_eff_dz{5.0f}; ///< Size of deadzone applied to negative baro innovations when ground effect compensation is active (m) + float ekf2_gnd_max_hgt{0.5f}; ///< Height above ground at which baro ground effect becomes insignificant (m) # if defined(CONFIG_EKF2_BARO_COMPENSATION) // static barometer pressure position error coefficient along body axes @@ -322,59 +322,55 @@ struct parameters { #if defined(CONFIG_EKF2_GNSS) int32_t ekf2_gps_ctrl {static_cast(GnssCtrl::HPOS) | static_cast(GnssCtrl::VEL)}; - float ekf2_gps_delay{110.0f}; ///< GPS measurement delay relative to the IMU (mSec) + float ekf2_gps_delay{110.0f}; ///< GPS measurement delay relative to the IMU (mSec) Vector3f gps_pos_body{}; ///< xyz position of the GPS antenna in body frame (m) // position and velocity fusion float ekf2_gps_v_noise{0.5f}; ///< minimum allowed observation noise for gps velocity fusion (m/sec) - float ekf2_gps_p_noise{0.5f}; ///< minimum allowed observation noise for gps position fusion (m) + float ekf2_gps_p_noise{0.5f}; ///< minimum allowed observation noise for gps position fusion (m) float gps_hgt_bias_nsd{0.13f}; ///< process noise for gnss height bias estimation (m/s/sqrt(Hz)) - float ekf2_gps_p_gate{5.0f}; ///< GPS horizontal position innovation consistency gate size (STD) - float ekf2_gps_v_gate{5.0f}; ///< GPS velocity innovation consistency gate size (STD) + float ekf2_gps_p_gate{5.0f}; ///< GPS horizontal position innovation consistency gate size (STD) + float ekf2_gps_v_gate{5.0f}; ///< GPS velocity innovation consistency gate size (STD) // these parameters control the strictness of GPS quality checks used to determine if the GPS is // good enough to set a local origin and commence aiding int32_t ekf2_gps_check{21}; ///< bitmask used to control which GPS quality checks are used - float ekf2_req_eph{5.0f}; ///< maximum acceptable horizontal position error (m) - float ekf2_req_epv{8.0f}; ///< maximum acceptable vertical position error (m) - float ekf2_req_sacc{1.0f}; ///< maximum acceptable speed error (m/s) - int32_t ekf2_req_nsats{6}; ///< minimum acceptable satellite count - float ekf2_req_pdop{2.0f}; ///< maximum acceptable position dilution of precision + float ekf2_req_eph{5.0f}; ///< maximum acceptable horizontal position error (m) + float ekf2_req_epv{8.0f}; ///< maximum acceptable vertical position error (m) + float ekf2_req_sacc{1.0f}; ///< maximum acceptable speed error (m/s) + int32_t ekf2_req_nsats{6}; ///< minimum acceptable satellite count + float ekf2_req_pdop{2.0f}; ///< maximum acceptable position dilution of precision float ekf2_req_hdrift{0.3f}; ///< maximum acceptable horizontal drift speed (m/s) float ekf2_req_vdrift{0.5f}; ///< maximum acceptable vertical drift speed (m/s) # if defined(CONFIG_EKF2_GNSS_YAW) // GNSS heading fusion - float gnss_heading_noise{0.1f}; ///< measurement noise standard deviation used for GNSS heading fusion (rad) + float gnss_heading_noise{0.1f}; ///< measurement noise standard deviation used for GNSS heading fusion (rad) # endif // CONFIG_EKF2_GNSS_YAW // Parameters used to control when yaw is reset to the EKF-GSF yaw estimator value - float ekf2_gsf_tas{15.0f}; ///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s) - const unsigned EKFGSF_reset_delay{1000000}; ///< Number of uSec of bad innovations on main filter in immediate post-takeoff phase before yaw is reset to EKF-GSF value - const float EKFGSF_yaw_err_max{0.262f}; ///< Composite yaw 1-sigma uncertainty threshold used to check for convergence (rad) + float ekf2_gsf_tas{15.0f}; ///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s) + const unsigned EKFGSF_reset_delay{1000000}; ///< Number of uSec of bad innovations on main filter in immediate post-takeoff phase before yaw is reset to EKF-GSF value + const float EKFGSF_yaw_err_max{0.262f}; ///< Composite yaw 1-sigma uncertainty threshold used to check for convergence (rad) #endif // CONFIG_EKF2_GNSS - float ekf2_noaid_noise{10.0f}; ///< observation noise for non-aiding position fusion (m) - - float ekf2_hdg_gate{2.6f}; ///< heading fusion innovation consistency gate size (STD) - float ekf2_head_noise{3.0e-1f}; ///< measurement noise used for simple heading fusion (rad) + float ekf2_noaid_noise{10.0f}; ///< observation noise for non-aiding position fusion (m) + float ekf2_hdg_gate{2.6f}; ///< heading fusion innovation consistency gate size (STD) + float ekf2_head_noise{3.0e-1f}; ///< measurement noise used for simple heading fusion (rad) #if defined(CONFIG_EKF2_MAGNETOMETER) - float ekf2_mag_delay {0.0f}; ///< magnetometer measurement delay relative to the IMU (mSec) - - float ekf2_mag_e_noise{1.0e-3f}; ///< process noise for earth magnetic field prediction (Gauss/sec) - float ekf2_mag_b_noise{1.0e-4f}; ///< process noise for body magnetic field prediction (Gauss/sec) - - // magnetometer fusion - float ekf2_mag_noise{5.0e-2f}; ///< measurement noise used for 3-axis magnetometer fusion (Gauss) - float ekf2_mag_decl{0.0f}; ///< magnetic declination (degrees) - float ekf2_mag_gate{3.0f}; ///< magnetometer fusion innovation consistency gate size (STD) - int32_t ekf2_decl_type{3}; ///< bitmask used to control the handling of declination data - int32_t ekf2_mag_type{0}; ///< integer used to specify the type of magnetometer fusion used - float ekf2_mag_acclim{0.5f}; ///< when in auto select mode, heading fusion will be used when manoeuvre accel is lower than this (m/sec**2) - + // Mag fusion + float ekf2_mag_delay{0.0f}; ///< magnetometer measurement delay relative to the IMU (mSec) + float ekf2_mag_e_noise{1.0e-3f}; ///< process noise for earth magnetic field prediction (Gauss/sec) + float ekf2_mag_b_noise{1.0e-4f}; ///< process noise for body magnetic field prediction (Gauss/sec) + float ekf2_mag_noise{5.0e-2f}; ///< measurement noise used for 3-axis magnetometer fusion (Gauss) + float ekf2_mag_decl{0.0f}; ///< magnetic declination (degrees) + float ekf2_mag_gate{3.0f}; ///< magnetometer fusion innovation consistency gate size (STD) + int32_t ekf2_decl_type{3}; ///< bitmask used to control the handling of declination data + int32_t ekf2_mag_type{0}; ///< integer used to specify the type of magnetometer fusion used + float ekf2_mag_acclim{0.5f}; ///< when in auto select mode, heading fusion will be used when manoeuvre accel is lower than this (m/sec**2) // compute synthetic magnetomter Z value if possible int32_t ekf2_synt_mag_z{0}; int32_t ekf2_mag_check{0}; @@ -384,46 +380,46 @@ struct parameters { #if defined(CONFIG_EKF2_AIRSPEED) // airspeed fusion - float ekf2_asp_delay{100.0f}; ///< airspeed measurement delay relative to the IMU (mSec) - float ekf2_tas_gate{5.0f}; ///< True Airspeed innovation consistency gate size (STD) - float ekf2_eas_noise{1.4f}; ///< EAS measurement noise standard deviation used for airspeed fusion (m/s) - float ekf2_arsp_thr{2.0f}; ///< Airspeed fusion threshold. A value of zero will deactivate airspeed fusion + float ekf2_asp_delay{100.0f}; ///< airspeed measurement delay relative to the IMU (mSec) + float ekf2_tas_gate{5.0f}; ///< True Airspeed innovation consistency gate size (STD) + float ekf2_eas_noise{1.4f}; ///< EAS measurement noise standard deviation used for airspeed fusion (m/s) + float ekf2_arsp_thr{2.0f}; ///< Airspeed fusion threshold. A value of zero will deactivate airspeed fusion #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_SIDESLIP) // synthetic sideslip fusion int32_t ekf2_fuse_beta{0}; - float ekf2_beta_gate{5.0f}; ///< synthetic sideslip innovation consistency gate size in standard deviation (STD) - float ekf2_beta_noise{0.3f}; ///< synthetic sideslip noise (rad) + float ekf2_beta_gate{5.0f}; ///< synthetic sideslip innovation consistency gate size in standard deviation (STD) + float ekf2_beta_noise{0.3f}; ///< synthetic sideslip noise (rad) const float beta_avg_ft_us{150000.0f}; ///< The average time between synthetic sideslip measurements (uSec) #endif // CONFIG_EKF2_SIDESLIP #if defined(CONFIG_EKF2_TERRAIN) - float terrain_p_noise {5.0f}; ///< process noise for terrain offset (m/sec) - float terrain_gradient{0.5f}; ///< gradient of terrain used to estimate process noise due to changing position (m/m) + // Terrain estimation + float ekf2_terr_noise{5.0f}; ///< process noise for terrain offset (m/sec) + float ekf2_terr_grad{0.5f}; ///< gradient of terrain used to estimate process noise due to changing position (m/m) const float terrain_timeout{10.f}; ///< maximum time for invalid bottom distance measurements before resetting terrain estimate (s) #endif // CONFIG_EKF2_TERRAIN #if defined(CONFIG_EKF2_TERRAIN) || defined(CONFIG_EKF2_OPTICAL_FLOW) || defined(CONFIG_EKF2_RANGE_FINDER) - float rng_gnd_clearance {0.1f}; ///< minimum valid value for range when on ground (m) + float ekf2_min_rng {0.1f}; ///< minimum valid value for range when on ground (m) #endif // CONFIG_EKF2_TERRAIN || CONFIG_EKF2_OPTICAL_FLOW || CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_RANGE_FINDER) // range finder fusion - int32_t rng_ctrl{static_cast(RngCtrl::CONDITIONAL)}; + int32_t ekf2_rng_ctrl{static_cast(RngCtrl::CONDITIONAL)}; - float range_delay_ms{5.0f}; ///< range finder measurement delay relative to the IMU (mSec) - float range_noise{0.1f}; ///< observation noise for range finder measurements (m) - float range_innov_gate{5.0f}; ///< range finder fusion innovation consistency gate size (STD) - float rng_sens_pitch{0.0f}; ///< Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis. - float range_noise_scaler{0.0f}; ///< scaling from range measurement to noise (m/m) - float max_hagl_for_range_aid{5.0f}; ///< maximum height above ground for which we allow to use the range finder as height source (if rng_control == 1) - float max_vel_for_range_aid{1.0f}; ///< maximum ground velocity for which we allow to use the range finder as height source (if rng_control == 1) - float range_aid_innov_gate{1.0f}; ///< gate size used for innovation consistency checks for range aid fusion - float range_valid_quality_s{1.0f}; ///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s) + float ekf2_rng_delay{5.0f}; ///< range finder measurement delay relative to the IMU (mSec) + float ekf2_rng_noise{0.1f}; ///< observation noise for range finder measurements (m) + float ekf2_rng_gate{5.0f}; ///< range finder fusion innovation consistency gate size (STD) + float ekf2_rng_pitch{0.0f}; ///< Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis. + float ekf2_rng_sfe{0.0f}; ///< scaling from range measurement to noise (m/m) + float ekf2_rng_a_hmax{5.0f}; ///< maximum height above ground for which we allow to use the range finder as height source (if rng_control == 1) + float ekf2_rng_a_vmax{1.0f}; ///< maximum ground velocity for which we allow to use the range finder as height source (if rng_control == 1) + float ekf2_rng_qlty_t{1.0f}; ///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s) float range_cos_max_tilt{0.7071f}; ///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data - float range_kin_consistency_gate{1.0f}; ///< gate size used by the range finder kinematic consistency check - float rng_fog{0.f}; ///< max distance which a blocked range sensor measures (fog, dirt) [m] + float ekf2_rng_k_gate{1.0f}; ///< gate size used by the range finder kinematic consistency check + float ekf2_rng_fog{0.f}; ///< max distance which a blocked range sensor measures (fog, dirt) [m] Vector3f rng_pos_body{}; ///< xyz position of range sensor in body frame (m) #endif // CONFIG_EKF2_RANGE_FINDER @@ -431,14 +427,13 @@ struct parameters { #if defined(CONFIG_EKF2_EXTERNAL_VISION) // vision position fusion int32_t ekf2_ev_ctrl{0}; - float ekf2_ev_delay{175.0f}; ///< off-board vision measurement delay relative to the IMU (mSec) - - float ekf2_evv_noise{0.1f}; ///< minimum allowed observation noise for EV velocity fusion (m/sec) - float ekf2_evp_noise{0.1f}; ///< minimum allowed observation noise for EV position fusion (m) - float ekf2_eva_noise{0.1f}; ///< minimum allowed observation noise for EV attitude fusion (rad/sec) - int32_t ekf2_ev_qmin{0}; ///< vision minimum acceptable quality integer - float ekf2_evv_gate{3.0f}; ///< vision velocity fusion innovation consistency gate size (STD) - float ekf2_evp_gate{5.0f}; ///< vision position fusion innovation consistency gate size (STD) + float ekf2_ev_delay{175.0f}; ///< off-board vision measurement delay relative to the IMU (mSec) + float ekf2_evv_noise{0.1f}; ///< minimum allowed observation noise for EV velocity fusion (m/sec) + float ekf2_evp_noise{0.1f}; ///< minimum allowed observation noise for EV position fusion (m) + float ekf2_eva_noise{0.1f}; ///< minimum allowed observation noise for EV attitude fusion (rad/sec) + int32_t ekf2_ev_qmin{0}; ///< vision minimum acceptable quality integer + float ekf2_evv_gate{3.0f}; ///< vision velocity fusion innovation consistency gate size (STD) + float ekf2_evp_gate{5.0f}; ///< vision position fusion innovation consistency gate size (STD) float ev_hgt_bias_nsd{0.13f}; ///< process noise for vision height bias estimation (m/s/sqrt(Hz)) Vector3f ev_pos_body{}; ///< xyz position of VI-sensor focal point in body frame (m) @@ -446,7 +441,7 @@ struct parameters { #if defined(CONFIG_EKF2_GRAVITY_FUSION) // gravity fusion - float ekf2_grav_noise{1.0f}; ///< accelerometer measurement gaussian noise (m/s**2) + float ekf2_grav_noise{1.0f}; ///< accelerometer measurement gaussian noise (m/s**2) #endif // CONFIG_EKF2_GRAVITY_FUSION #if defined(CONFIG_EKF2_OPTICAL_FLOW) @@ -455,11 +450,11 @@ struct parameters { float ekf2_of_delay{5.0f}; ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval // optical flow fusion - float ekf2_of_n_min{0.15f}; ///< observation noise for optical flow LOS rate measurements (rad/sec) - float ekf2_of_n_max{0.5f}; ///< observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec) - int32_t ekf2_of_qmin{1}; ///< minimum acceptable quality integer from the flow sensor - int32_t ekf2_of_qmin_gnd{0}; ///< minimum acceptable quality integer from the flow sensor when on ground - float ekf2_of_gate{3.0f}; ///< optical flow fusion innovation consistency gate size (STD) + float ekf2_of_n_min{0.15f}; ///< observation noise for optical flow LOS rate measurements (rad/sec) + float ekf2_of_n_max{0.5f}; ///< observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec) + int32_t ekf2_of_qmin{1}; ///< minimum acceptable quality integer from the flow sensor + int32_t ekf2_of_qmin_gnd{0}; ///< minimum acceptable quality integer from the flow sensor when on ground + float ekf2_of_gate{3.0f}; ///< optical flow fusion innovation consistency gate size (STD) Vector3f flow_pos_body{}; ///< xyz position of range sensor focal point in body frame (m) #endif // CONFIG_EKF2_OPTICAL_FLOW @@ -469,11 +464,11 @@ struct parameters { // accel bias learning control float ekf2_abl_lim{0.4f}; ///< maximum accel bias magnitude (m/sec**2) - float ekf2_abl_acclim{25.0f}; ///< learning is disabled if the magnitude of the IMU acceleration vector is greater than this (m/sec**2) - float ekf2_abl_gyrlim{3.0f}; ///< learning is disabled if the magnitude of the IMU angular rate vector is greater than this (rad/sec) - float ekf2_abl_tau{0.5f}; ///< time constant used to control the decaying envelope filters applied to the accel and gyro magnitudes (sec) + float ekf2_abl_acclim{25.0f}; ///< learning is disabled if the magnitude of the IMU acceleration vector is greater than this (m/sec**2) + float ekf2_abl_gyrlim{3.0f}; ///< learning is disabled if the magnitude of the IMU angular rate vector is greater than this (rad/sec) + float ekf2_abl_tau{0.5f}; ///< time constant used to control the decaying envelope filters applied to the accel and gyro magnitudes (sec) - float ekf2_gyr_b_lim{0.4f}; ///< maximum gyro bias magnitude (rad/sec) + float ekf2_gyr_b_lim{0.4f}; ///< maximum gyro bias magnitude (rad/sec) const unsigned reset_timeout_max{7'000'000}; ///< maximum time we allow horizontal inertial dead reckoning before attempting to reset the states to the measurement or change _control_status if the data is unavailable (uSec) const unsigned no_aid_timeout_max{1'000'000}; ///< maximum lapsed time from last fusion of a measurement that constrains horizontal velocity drift before the EKF will determine that the sensor is no longer contributing to aiding (uSec) @@ -484,16 +479,16 @@ struct parameters { #if defined(CONFIG_EKF2_DRAG_FUSION) // multi-rotor drag specific force fusion int32_t ekf2_drag_ctrl{0}; - float ekf2_drag_noise{2.5f}; ///< observation noise variance for drag specific force measurements (m/sec**2)**2 - float ekf2_bcoef_x{100.0f}; ///< bluff body drag ballistic coefficient for the X-axis (kg/m**2) - float ekf2_bcoef_y{100.0f}; ///< bluff body drag ballistic coefficient for the Y-axis (kg/m**2) - float ekf2_mcoef{0.1f}; ///< rotor momentum drag coefficient for the X and Y axes (1/s) + float ekf2_drag_noise{2.5f}; ///< observation noise variance for drag specific force measurements (m/sec**2)**2 + float ekf2_bcoef_x{100.0f}; ///< bluff body drag ballistic coefficient for the X-axis (kg/m**2) + float ekf2_bcoef_y{100.0f}; ///< bluff body drag ballistic coefficient for the Y-axis (kg/m**2) + float ekf2_mcoef{0.1f}; ///< rotor momentum drag coefficient for the X and Y axes (1/s) #endif // CONFIG_EKF2_DRAG_FUSION // control of accel error detection and mitigation (IMU clipping) - const float vert_innov_test_lim{3.0f}; ///< Number of standard deviations of vertical vel/pos innovations allowed before triggering a vertical acceleration failure - const float vert_innov_test_min{1.0f}; ///< Minimum number of standard deviations of vertical vel/pos innovations required to trigger a vertical acceleration failure - const int bad_acc_reset_delay_us{500000}; ///< Continuous time that the vertical position and velocity innovation test must fail before the states are reset (uSec) + const float vert_innov_test_lim{3.0f}; ///< Number of standard deviations of vertical vel/pos innovations allowed before triggering a vertical acceleration failure + const float vert_innov_test_min{1.0f}; ///< Minimum number of standard deviations of vertical vel/pos innovations required to trigger a vertical acceleration failure + const int bad_acc_reset_delay_us{500000}; ///< Continuous time that the vertical position and velocity innovation test must fail before the states are reset (uSec) #if defined(CONFIG_EKF2_AUXVEL) // auxiliary velocity fusion @@ -635,7 +630,7 @@ bool yaw_aligned_to_imu_gps : bool reset_hgt_to_ev : 1; ///< 16 - true when the vertical position state is reset to the ev measurement bool reset_pos_to_ext_obs : 1; ///< 17 - true when horizontal position was reset to an external observation while deadreckoning - bool reset_wind_to_ext_obs : 1; ///< 18 - true when wind states were reset to an external observation + bool reset_wind_to_ext_obs : 1; ///< 18 - true when wind states were reset to an external observation } flags; uint32_t value; }; diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 8cb9e4be67..0998f86b43 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -156,7 +156,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) _zero_velocity_update.update(*this, imu_delayed); - if (_params.imu_ctrl & static_cast(ImuCtrl::GyroBias)) { + if (_params.ekf2_imu_ctrl & static_cast(ImuCtrl::GyroBias)) { _zero_gyro_update.update(*this, imu_delayed); } diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index c42d408592..85ce9b9c17 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -84,7 +84,7 @@ void Ekf::initialiseCovariance() #if defined(CONFIG_EKF2_RANGE_FINDER) if (_control_status.flags.rng_hgt) { - z_pos_var = sq(fmaxf(_params.range_noise, 0.01f)); + z_pos_var = sq(fmaxf(_params.ekf2_rng_noise, 0.01f)); } #endif // CONFIG_EKF2_RANGE_FINDER @@ -106,7 +106,7 @@ void Ekf::initialiseCovariance() #if defined(CONFIG_EKF2_TERRAIN) // use the ground clearance value as our uncertainty - P.uncorrelateCovarianceSetVariance(State::terrain.idx, sq(_params.rng_gnd_clearance)); + P.uncorrelateCovarianceSetVariance(State::terrain.idx, sq(_params.ekf2_min_rng)); #endif // CONFIG_EKF2_TERRAIN } @@ -221,10 +221,10 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) if (_height_sensor_ref != HeightSensor::RANGE) { // predict the state variance growth where the state is the vertical position of the terrain underneath the vehicle // process noise due to errors in vehicle height estimate - float terrain_process_noise = sq(imu_delayed.delta_vel_dt * _params.terrain_p_noise); + float terrain_process_noise = sq(imu_delayed.delta_vel_dt * _params.ekf2_terr_noise); // process noise due to terrain gradient - terrain_process_noise += sq(imu_delayed.delta_vel_dt * _params.terrain_gradient) * (sq(_state.vel(0)) + sq(_state.vel( + terrain_process_noise += sq(imu_delayed.delta_vel_dt * _params.ekf2_terr_grad) * (sq(_state.vel(0)) + sq(_state.vel( 1))); P(State::terrain.idx, State::terrain.idx) += terrain_process_noise; } diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index b5c8b044fb..8e617bdb63 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -74,14 +74,14 @@ void Ekf::reset() // #if defined(CONFIG_EKF2_TERRAIN) // assume a ground clearance - _state.terrain = -_gpos.altitude() + _params.rng_gnd_clearance; + _state.terrain = -_gpos.altitude() + _params.ekf2_min_rng; #endif // CONFIG_EKF2_TERRAIN #if defined(CONFIG_EKF2_RANGE_FINDER) - _range_sensor.setPitchOffset(_params.rng_sens_pitch); + _range_sensor.setPitchOffset(_params.ekf2_rng_pitch); _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt); - _range_sensor.setQualityHysteresis(_params.range_valid_quality_s); - _range_sensor.setMaxFogDistance(_params.rng_fog); + _range_sensor.setQualityHysteresis(_params.ekf2_rng_qlty_t); + _range_sensor.setMaxFogDistance(_params.ekf2_rng_fog); #endif // CONFIG_EKF2_RANGE_FINDER _control_status.value = 0; diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 4f393bb499..5820494f8e 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -294,7 +294,7 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const #if defined(CONFIG_EKF2_OPTICAL_FLOW) if (_control_status.flags.opt_flow) { - float gndclearance = math::max(_params.rng_gnd_clearance, 0.1f); + float gndclearance = math::max(_params.ekf2_min_rng, 0.1f); vel_err_conservative = math::max(getHagl(), gndclearance) * Vector2f(_aid_src_optical_flow.innovation).norm(); } @@ -996,7 +996,7 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed) // gyro bias inhibit - const bool do_inhibit_all_gyro_axes = !(_params.imu_ctrl & static_cast(ImuCtrl::GyroBias)); + const bool do_inhibit_all_gyro_axes = !(_params.ekf2_imu_ctrl & static_cast(ImuCtrl::GyroBias)); for (unsigned index = 0; index < State::gyro_bias.dof; index++) { bool is_bias_observable = true; // TODO: gyro bias conditions @@ -1004,7 +1004,7 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed) } // accel bias inhibit - const bool do_inhibit_all_accel_axes = !(_params.imu_ctrl & static_cast(ImuCtrl::AccelBias)) + const bool do_inhibit_all_accel_axes = !(_params.ekf2_imu_ctrl & static_cast(ImuCtrl::AccelBias)) || is_manoeuvre_level_high || _fault_status.flags.bad_acc_vertical; diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 945310b07f..9e0ebec771 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -302,7 +302,7 @@ void EstimatorInterface::setRangeData(const sensor::rangeSample &range_sample) } const int64_t time_us = range_sample.time_us - - static_cast(_params.range_delay_ms * 1000) + - static_cast(_params.ekf2_rng_delay * 1000) - static_cast(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2 // limit data rate to prevent data being lost diff --git a/src/modules/ekf2/EKF/height_control.cpp b/src/modules/ekf2/EKF/height_control.cpp index 44467fa2b8..bc7f95f0e3 100644 --- a/src/modules/ekf2/EKF/height_control.cpp +++ b/src/modules/ekf2/EKF/height_control.cpp @@ -67,7 +67,7 @@ void Ekf::checkHeightSensorRefFallback() HeightSensor fallback_list[4]; - switch (static_cast(_params.height_sensor_ref)) { + switch (static_cast(_params.ekf2_hgt_ref)) { default: /* FALLTHROUGH */ diff --git a/src/modules/ekf2/EKF/terrain_control.cpp b/src/modules/ekf2/EKF/terrain_control.cpp index d005022289..df3b88cc67 100644 --- a/src/modules/ekf2/EKF/terrain_control.cpp +++ b/src/modules/ekf2/EKF/terrain_control.cpp @@ -43,10 +43,10 @@ void Ekf::initTerrain() { // assume a ground clearance - _state.terrain = -_gpos.altitude() + _params.rng_gnd_clearance; + _state.terrain = -_gpos.altitude() + _params.ekf2_min_rng; // use the ground clearance value as our uncertainty - P.uncorrelateCovarianceSetVariance(State::terrain.idx, sq(_params.rng_gnd_clearance)); + P.uncorrelateCovarianceSetVariance(State::terrain.idx, sq(_params.ekf2_min_rng)); } void Ekf::controlTerrainFakeFusion() diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 547e9fee9b..785f5565f4 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -65,7 +65,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _params(_ekf.getParamHandle()), _param_ekf2_predict_us(_params->ekf2_predict_us), _param_ekf2_delay_max(_params->ekf2_delay_max), - _param_ekf2_imu_ctrl(_params->imu_ctrl), + _param_ekf2_imu_ctrl(_params->ekf2_imu_ctrl), _param_ekf2_vel_lim(_params->ekf2_vel_lim), #if defined(CONFIG_EKF2_AUXVEL) _param_ekf2_avel_delay(_params->ekf2_avel_delay), @@ -142,28 +142,27 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_mag_chk_inc(_params->ekf2_mag_chk_inc), _param_ekf2_synt_mag_z(_params->ekf2_synt_mag_z), #endif // CONFIG_EKF2_MAGNETOMETER - _param_ekf2_hgt_ref(_params->height_sensor_ref), + _param_ekf2_hgt_ref(_params->ekf2_hgt_ref), _param_ekf2_noaid_tout(_params->ekf2_noaid_tout), #if defined(CONFIG_EKF2_TERRAIN) || defined(CONFIG_EKF2_OPTICAL_FLOW) || defined(CONFIG_EKF2_RANGE_FINDER) - _param_ekf2_min_rng(_params->rng_gnd_clearance), + _param_ekf2_min_rng(_params->ekf2_min_rng), #endif // CONFIG_EKF2_TERRAIN || CONFIG_EKF2_OPTICAL_FLOW || CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_TERRAIN) - _param_ekf2_terr_noise(_params->terrain_p_noise), - _param_ekf2_terr_grad(_params->terrain_gradient), + _param_ekf2_terr_noise(_params->ekf2_terr_noise), + _param_ekf2_terr_grad(_params->ekf2_terr_grad), #endif // CONFIG_EKF2_TERRAIN #if defined(CONFIG_EKF2_RANGE_FINDER) - _param_ekf2_rng_ctrl(_params->rng_ctrl), - _param_ekf2_rng_delay(_params->range_delay_ms), - _param_ekf2_rng_noise(_params->range_noise), - _param_ekf2_rng_sfe(_params->range_noise_scaler), - _param_ekf2_rng_gate(_params->range_innov_gate), - _param_ekf2_rng_pitch(_params->rng_sens_pitch), - _param_ekf2_rng_a_vmax(_params->max_vel_for_range_aid), - _param_ekf2_rng_a_hmax(_params->max_hagl_for_range_aid), - _param_ekf2_rng_a_igate(_params->range_aid_innov_gate), - _param_ekf2_rng_qlty_t(_params->range_valid_quality_s), - _param_ekf2_rng_k_gate(_params->range_kin_consistency_gate), - _param_ekf2_rng_fog(_params->rng_fog), + _param_ekf2_rng_ctrl(_params->ekf2_rng_ctrl), + _param_ekf2_rng_delay(_params->ekf2_rng_delay), + _param_ekf2_rng_noise(_params->ekf2_rng_noise), + _param_ekf2_rng_sfe(_params->ekf2_rng_sfe), + _param_ekf2_rng_gate(_params->ekf2_rng_gate), + _param_ekf2_rng_pitch(_params->ekf2_rng_pitch), + _param_ekf2_rng_a_vmax(_params->ekf2_rng_a_vmax), + _param_ekf2_rng_a_hmax(_params->ekf2_rng_a_hmax), + _param_ekf2_rng_qlty_t(_params->ekf2_rng_qlty_t), + _param_ekf2_rng_k_gate(_params->ekf2_rng_k_gate), + _param_ekf2_rng_fog(_params->ekf2_rng_fog), _param_ekf2_rng_pos_x(_params->rng_pos_body(0)), _param_ekf2_rng_pos_y(_params->rng_pos_body(1)), _param_ekf2_rng_pos_z(_params->rng_pos_body(2)), diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index bf60078e8f..dcae0b0cf3 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -618,7 +618,6 @@ private: (ParamExtFloat) _param_ekf2_rng_pitch, (ParamExtFloat) _param_ekf2_rng_a_vmax, (ParamExtFloat) _param_ekf2_rng_a_hmax, - (ParamExtFloat) _param_ekf2_rng_a_igate, (ParamExtFloat) _param_ekf2_rng_qlty_t, (ParamExtFloat) _param_ekf2_rng_k_gate, (ParamExtFloat) _param_ekf2_rng_fog, diff --git a/src/modules/ekf2/params_range_finder.yaml b/src/modules/ekf2/params_range_finder.yaml index 97f60bdff4..3f1a47ff7b 100644 --- a/src/modules/ekf2/params_range_finder.yaml +++ b/src/modules/ekf2/params_range_finder.yaml @@ -83,16 +83,6 @@ parameters: min: 1.0 max: 10.0 unit: m - EKF2_RNG_A_IGATE: - description: - short: Gate size used for innovation consistency checks for range aid fusion - long: A lower value means HAGL needs to be more stable in order to use range - finder for height estimation in range aid mode - type: float - default: 1.0 - unit: SD - min: 0.1 - max: 5.0 EKF2_RNG_QLTY_T: description: short: Minumum range validity period diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp index 4787be55fa..1943f5015e 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp @@ -12,7 +12,7 @@ EkfWrapper::~EkfWrapper() void EkfWrapper::setBaroHeightRef() { - _ekf_params->height_sensor_ref = static_cast(HeightSensor::BARO); + _ekf_params->ekf2_hgt_ref = static_cast(HeightSensor::BARO); } void EkfWrapper::enableBaroHeightFusion() @@ -32,7 +32,7 @@ bool EkfWrapper::isIntendingBaroHeightFusion() const void EkfWrapper::setGpsHeightRef() { - _ekf_params->height_sensor_ref = static_cast(HeightSensor::GNSS); + _ekf_params->ekf2_hgt_ref = static_cast(HeightSensor::GNSS); } void EkfWrapper::enableGpsHeightFusion() @@ -52,17 +52,17 @@ bool EkfWrapper::isIntendingGpsHeightFusion() const void EkfWrapper::setRangeHeightRef() { - _ekf_params->height_sensor_ref = static_cast(HeightSensor::RANGE); + _ekf_params->ekf2_hgt_ref = static_cast(HeightSensor::RANGE); } void EkfWrapper::enableRangeHeightFusion() { - _ekf_params->rng_ctrl = static_cast(RngCtrl::ENABLED); + _ekf_params->ekf2_rng_ctrl = static_cast(RngCtrl::ENABLED); } void EkfWrapper::disableRangeHeightFusion() { - _ekf_params->rng_ctrl = static_cast(RngCtrl::DISABLED); + _ekf_params->ekf2_rng_ctrl = static_cast(RngCtrl::DISABLED); } bool EkfWrapper::isIntendingRangeHeightFusion() const @@ -72,7 +72,7 @@ bool EkfWrapper::isIntendingRangeHeightFusion() const void EkfWrapper::setExternalVisionHeightRef() { - _ekf_params->height_sensor_ref = static_cast(HeightSensor::EV); + _ekf_params->ekf2_hgt_ref = static_cast(HeightSensor::EV); } void EkfWrapper::enableExternalVisionHeightFusion() @@ -293,10 +293,10 @@ float EkfWrapper::getMagHeadingNoise() const void EkfWrapper::enableGyroBiasEstimation() { - _ekf_params->imu_ctrl |= static_cast(ImuCtrl::GyroBias); + _ekf_params->ekf2_imu_ctrl |= static_cast(ImuCtrl::GyroBias); } void EkfWrapper::disableGyroBiasEstimation() { - _ekf_params->imu_ctrl &= ~static_cast(ImuCtrl::GyroBias); + _ekf_params->ekf2_imu_ctrl &= ~static_cast(ImuCtrl::GyroBias); }