From e57af44d71dea2da62055076f186b3d15278ae7e Mon Sep 17 00:00:00 2001 From: Bazooka Joe Date: Tue, 18 May 2021 20:06:32 +0300 Subject: [PATCH] set parameters that doesn't change with const attribute, for clearification --- EKF/common.h | 34 +++++++++++++++++----------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 051073534e..67ce472be3 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -240,16 +240,16 @@ struct parameters { 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) float wind_vel_p_noise{1.0e-1f}; ///< process noise for wind velocity prediction (m/sec**2) - float wind_vel_p_noise_scaler{0.5f}; ///< scaling of wind process noise with vertical velocity + const float wind_vel_p_noise_scaler{0.5f}; ///< scaling of wind process noise with vertical velocity 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) - float terrain_timeout{10.f}; ///< maximum time for invalid bottom distance measurements before resetting terrain estimate (s) + const float terrain_timeout{10.f}; ///< maximum time for invalid bottom distance measurements before resetting terrain estimate (s) // 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) - float initial_wind_uncertainty{1.0f}; ///< 1-sigma initial uncertainty in wind velocity (m/sec) + const float initial_wind_uncertainty{1.0f}; ///< 1-sigma initial uncertainty in wind velocity (m/sec) // position and velocity fusion float gps_vel_noise{5.0e-1f}; ///< minimum allowed observation noise for gps velocity fusion (m/sec) @@ -272,7 +272,7 @@ struct parameters { int32_t mag_fusion_type{0}; ///< integer used to specify the type of magnetometer fusion used float mag_acc_gate{0.5f}; ///< when in auto select mode, heading fusion will be used when manoeuvre accel is lower than this (m/sec**2) float mag_yaw_rate_gate{0.25f}; ///< yaw rate threshold used by mode select logic (rad/sec) - float quat_max_variance{0.0001f}; ///< zero innovation yaw measurements will not be fused when the sum of quaternion variance is less than this + const float quat_max_variance{0.0001f}; ///< zero innovation yaw measurements will not be fused when the sum of quaternion variance is less than this // airspeed fusion float tas_innov_gate{5.0f}; ///< True Airspeed innovation consistency gate size (STD) @@ -281,7 +281,7 @@ struct parameters { // synthetic sideslip fusion float beta_innov_gate{5.0f}; ///< synthetic sideslip innovation consistency gate size in standard deviation (STD) float beta_noise{0.3f}; ///< synthetic sideslip noise (rad) - float beta_avg_ft_us{150000.0f}; ///< The average time between synthetic sideslip measurements (uSec) + const float beta_avg_ft_us{150000.0f}; ///< The average time between synthetic sideslip measurements (uSec) // range finder fusion float range_noise{0.1f}; ///< observation noise for range finder measurements (m) @@ -289,7 +289,7 @@ struct parameters { float rng_gnd_clearance{0.1f}; ///< minimum valid value for range when on ground (m) 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 vehicle_variance_scaler{0.0f}; ///< gain applied to vehicle height variance used in calculation of height above ground observation variance + const float vehicle_variance_scaler{0.0f}; ///< gain applied to vehicle height variance used in calculation of height above ground observation variance 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 range_aid == 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 range_aid == 1) int32_t range_aid{0}; ///< allow switching primary height source to range finder if certain conditions are met @@ -298,8 +298,8 @@ struct parameters { 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 // vision position fusion - float ev_vel_innov_gate{3.0f}; ///< vision velocity fusion innovation consistency gate size (STD) - float ev_pos_innov_gate{5.0f}; ///< vision position fusion innovation consistency gate size (STD) + float ev_vel_innov_gate{3.0f}; ///< vision velocity fusion innovation consistency gate size (STD) + float ev_pos_innov_gate{5.0f}; ///< vision position fusion innovation consistency gate size (STD) // optical flow fusion float flow_noise{0.15f}; ///< observation noise for optical flow LOS rate measurements (rad/sec) @@ -335,8 +335,8 @@ struct parameters { float acc_bias_learn_gyr_lim{3.0f}; ///< learning is disabled if the magnitude of the IMU angular rate vector is greater than this (rad/sec) float acc_bias_learn_tc{0.5f}; ///< time constant used to control the decaying envelope filters applied to the accel and gyro magnitudes (sec) - unsigned reset_timeout_max{7000000}; ///< 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) - unsigned no_aid_timeout_max{1000000}; ///< 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) + const unsigned reset_timeout_max{7000000}; ///< 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{1000000}; ///< 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) int32_t valid_timeout_max{5000000}; ///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec) @@ -355,12 +355,12 @@ struct parameters { float bcoef_y{25.0f}; ///< ballistic coefficient along the Y-axis (kg/m**2) // control of accel error detection and mitigation (IMU clipping) - float vert_innov_test_lim{3.0f}; ///< Number of standard deviations allowed before the combined vertical velocity and position test is declared as failed - 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 allowed before the combined vertical velocity and position test is declared as failed + 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) // auxiliary velocity fusion - float auxvel_noise{0.5f}; ///< minimum observation noise, uses reported noise if greater (m/s) - float auxvel_gate{5.0f}; ///< velocity fusion innovation consistency gate size (STD) + const float auxvel_noise{0.5f}; ///< minimum observation noise, uses reported noise if greater (m/s) + const float auxvel_gate{5.0f}; ///< velocity fusion innovation consistency gate size (STD) // control of on-ground movement check float is_moving_scaler{1.0f}; ///< gain scaler used to adjust the threshold for the on-ground movement detection. Larger values make the test less sensitive. @@ -371,9 +371,9 @@ struct parameters { // Parameters used to control when yaw is reset to the EKF-GSF yaw estimator value float EKFGSF_tas_default{15.0f}; ///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s) - 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 - float EKFGSF_yaw_err_max{0.262f}; ///< Composite yaw 1-sigma uncertainty threshold used to check for convergence (rad) - unsigned EKFGSF_reset_count_limit{3}; ///< Maximum number of times the yaw can be reset to the EKF-GSF yaw estimator value + 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) + const unsigned EKFGSF_reset_count_limit{3}; ///< Maximum number of times the yaw can be reset to the EKF-GSF yaw estimator value }; struct stateSample {