floateas2tas{};///< equivalent to true airspeed factor
};
structflowSample{
uint64_ttime_us{0};///< timestamp of the integration period leading edge (uSec)
Vector2fflow_xy_rad;///< measured delta angle of the image about the X and Y body axes (rad), RH rotation is positive
Vector3fgyro_xyz;///< measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (rad), RH rotation is positive
floatdt;///< amount of integration time (sec)
uint8_tquality;///< quality indicator between 0 and 255
uint64_ttime_us{};///< timestamp of the integration period leading edge (uSec)
Vector2fflow_xy_rad{};///< measured delta angle of the image about the X and Y body axes (rad), RH rotation is positive
Vector3fgyro_xyz{};///< measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (rad), RH rotation is positive
floatdt{};///< amount of integration time (sec)
uint8_tquality{};///< quality indicator between 0 and 255
};
structextVisionSample{
uint64_ttime_us{0};///< timestamp of the measurement (uSec)
Vector3fpos;///< XYZ position in external vision's local reference frame (m) - Z must be aligned with down axis
Vector3fvel;///< FRD velocity in reference frame defined in vel_frame variable (m/sec) - Z must be aligned with down axis
Quatfquat;///< quaternion defining rotation from body to earth frame
uint64_ttime_us{0};///< timestamp of the measurement (uSec)
Vector2faccelXY;///< measured specific force along the X and Y body axes (m/sec**2)
uint64_ttime_us{};///< timestamp of the measurement (uSec)
Vector2faccelXY{};///< measured specific force along the X and Y body axes (m/sec**2)
};
structauxVelSample{
uint64_ttime_us{0};///< timestamp of the measurement (uSec)
Vector3fvel;///< measured NE velocity relative to the local origin (m/sec)
Vector3fvelVar;///< estimated error variance of the NE velocity (m/sec)**2
uint64_ttime_us{};///< timestamp of the measurement (uSec)
Vector3fvel{};///< measured NE velocity relative to the local origin (m/sec)
Vector3fvelVar{};///< estimated error variance of the NE velocity (m/sec)**2
};
// Integer definitions for vdist_sensor_type
#define VDIST_SENSOR_BARO 0///< Use baro height
#define VDIST_SENSOR_GPS 1///< Use GPS height
#define VDIST_SENSOR_RANGE 2///< Use range finder height
#define VDIST_SENSOR_BARO 0///< Use baro height
#define VDIST_SENSOR_GPS 1///< Use GPS height
#define VDIST_SENSOR_RANGE 2///< Use range finder height
#define VDIST_SENSOR_EV 3 ///< Use external vision
// Bit locations for mag_declination_source
#define MASK_USE_GEO_DECL (1<<0) ///< set to true to use the declination from the geo library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value
#define MASK_SAVE_GEO_DECL (1<<1) ///< set to true to set the EKF2_MAG_DECL parameter to the value returned by the geo library
#define MASK_FUSE_DECL (1<<2) ///< set to true if the declination is always fused as an observation to constrain drift when 3-axis fusion is performed
#define MASK_USE_GEO_DECL (1<<0)///< set to true to use the declination from the geo library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value
#define MASK_SAVE_GEO_DECL (1<<1)///< set to true to set the EKF2_MAG_DECL parameter to the value returned by the geo library
#define MASK_FUSE_DECL (1<<2)///< set to true if the declination is always fused as an observation to constrain drift when 3-axis fusion is performed
// Bit locations for fusion_mode
#define MASK_USE_GPS (1<<0) ///< set to true to use GPS data
#define MASK_USE_OF (1<<1) ///< set to true to use optical flow data
#define MASK_INHIBIT_ACC_BIAS (1<<2)///< set to true to inhibit estimation of accelerometer delta velocity bias
#define MASK_USE_EVPOS (1<<3) ///< set to true to use external vision position data
#define MASK_USE_EVYAW (1<<4) ///< set to true to use external vision quaternion data for yaw
#define MASK_USE_DRAG (1<<5) ///< set to true to use the multi-rotor drag model to estimate wind
#define MASK_ROTATE_EV (1<<6) ///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used
#define MASK_USE_GPSYAW (1<<7) ///< set to true to use GPS yaw data if available
#define MASK_USE_EVVEL (1<<8) ///< set to true to use external vision velocity data
#define MASK_USE_GPS (1<<0) ///< set to true to use GPS data
#define MASK_USE_OF (1<<1) ///< set to true to use optical flow data
#define MASK_INHIBIT_ACC_BIAS (1<<2)///< set to true to inhibit estimation of accelerometer delta velocity bias
#define MASK_USE_EVPOS (1<<3) ///< set to true to use external vision position data
#define MASK_USE_EVYAW (1<<4) ///< set to true to use external vision quaternion data for yaw
#define MASK_USE_DRAG (1<<5) ///< set to true to use the multi-rotor drag model to estimate wind
#define MASK_ROTATE_EV (1<<6) ///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used
#define MASK_USE_GPSYAW (1<<7) ///< set to true to use GPS yaw data if available
#define MASK_USE_EVVEL (1<<8) ///< set to true to use external vision velocity data
#define MAG_FUSE_TYPE_AUTO 0 ///< The selection of either heading or 3D magnetometer fusion will be automatic
#define MAG_FUSE_TYPE_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
#define MAG_FUSE_TYPE_3D 2 ///< Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions
#define MAG_FUSE_TYPE_UNUSED 3 ///< Not implemented
#define MAG_FUSE_TYPE_INDOOR 4 ///< The same as option 0, but magnetometer or yaw fusion will not be used unless earth frame external aiding (GPS or External Vision) is being used. This prevents inconsistent magnetic fields associated with indoor operation degrading state estimates.
#define MAG_FUSE_TYPE_NONE 5 ///< Do not use magnetometer under any circumstance. Other sources of yaw may be used if selected via the EKF2_AID_MASK parameter.
#define MAG_FUSE_TYPE_AUTO 0 ///< The selection of either heading or 3D magnetometer fusion will be automatic
#define MAG_FUSE_TYPE_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
#define MAG_FUSE_TYPE_3D 2 ///< Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions
#define MAG_FUSE_TYPE_UNUSED 3 ///< Not implemented
#define MAG_FUSE_TYPE_INDOOR 4 ///< The same as option 0, but magnetometer or yaw fusion will not be used unless earth frame external aiding (GPS or External Vision) is being used. This prevents inconsistent magnetic fields associated with indoor operation degrading state estimates.
#define MAG_FUSE_TYPE_NONE 5 ///< Do not use magnetometer under any circumstance. Other sources of yaw may be used if selected via the EKF2_AID_MASK parameter.
// Maximum sensor intervals in usec
#define GPS_MAX_INTERVAL (uint64_t)5e5///< Maximum allowable time interval between GPS measurements (uSec)
#define BARO_MAX_INTERVAL (uint64_t)2e5///< Maximum allowable time interval between pressure altitude measurements (uSec)
#define RNG_MAX_INTERVAL (uint64_t)2e5///< Maximum allowable time interval between range finder measurements (uSec)
#define EV_MAX_INTERVAL (uint64_t)2e5///< Maximum allowable time interval between external vision system measurements (uSec)
#define GPS_MAX_INTERVAL (uint64_t)5e5///< Maximum allowable time interval between GPS measurements (uSec)
#define BARO_MAX_INTERVAL (uint64_t)2e5///< Maximum allowable time interval between pressure altitude measurements (uSec)
#define RNG_MAX_INTERVAL (uint64_t)2e5///< Maximum allowable time interval between range finder measurements (uSec)
#define EV_MAX_INTERVAL (uint64_t)2e5///< Maximum allowable time interval between external vision system measurements (uSec)
// bad accelerometer detection and mitigation
#define BADACC_PROBATION (uint64_t)10e6///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec)
#define BADACC_BIAS_PNOISE 4.9f ///< The delta velocity process noise is set to this when accel data is declared bad (m/sec**2)
#define BADACC_PROBATION (uint64_t)10e6///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec)
#define BADACC_BIAS_PNOISE 4.9f ///< The delta velocity process noise is set to this when accel data is declared bad (m/sec**2)
// ground effect compensation
#define GNDEFFECT_TIMEOUT 10E6 ///< Maximum period of time that ground effect protection will be active after it was last turned on (uSec)
#define GNDEFFECT_TIMEOUT 10E6 ///< Maximum period of time that ground effect protection will be active after it was last turned on (uSec)
structparameters{
int32_tfilter_update_interval_us{10000};///< filter update interval in microseconds
// measurement source control
int32_tfusion_mode{MASK_USE_GPS};///< bitmasked integer that selects which aiding sources will be used
int32_tvdist_sensor_type{VDIST_SENSOR_BARO};///< selects the primary source for height data
int32_tfusion_mode{MASK_USE_GPS};///< bitmasked integer that selects which aiding sources will be used
int32_tvdist_sensor_type{VDIST_SENSOR_BARO};///< selects the primary source for height data
TerrainFusionMask::TerrainFuseOpticalFlow};///< aiding source(s) selection bitmask for the terrain estimator
int32_tsensor_interval_max_ms{10};///< maximum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers. (mSec)
int32_tsensor_interval_max_ms{10};///< maximum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers. (mSec)
// measurement time delays
floatmag_delay_ms{0.0f};///< magnetometer measurement delay relative to the IMU (mSec)
floatbaro_delay_ms{0.0f};///< barometer height measurement delay relative to the IMU (mSec)
floatgps_delay_ms{110.0f};///< GPS measurement delay relative to the IMU (mSec)
floatairspeed_delay_ms{100.0f};///< airspeed measurement delay relative to the IMU (mSec)
floatflow_delay_ms{5.0f};///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
floatrange_delay_ms{5.0f};///< range finder measurement delay relative to the IMU (mSec)
floatev_delay_ms{175.0f};///< off-board vision measurement delay relative to the IMU (mSec)
floatauxvel_delay_ms{5.0f};///< auxiliary velocity measurement delay relative to the IMU (mSec)
floatmag_delay_ms{0.0f};///< magnetometer measurement delay relative to the IMU (mSec)
floatbaro_delay_ms{0.0f};///< barometer height measurement delay relative to the IMU (mSec)
floatgps_delay_ms{110.0f};///< GPS measurement delay relative to the IMU (mSec)
floatairspeed_delay_ms{100.0f};///< airspeed measurement delay relative to the IMU (mSec)
floatflow_delay_ms{5.0f};///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
floatrange_delay_ms{5.0f};///< range finder measurement delay relative to the IMU (mSec)
floatev_delay_ms{175.0f};///< off-board vision measurement delay relative to the IMU (mSec)
floatauxvel_delay_ms{5.0f};///< auxiliary velocity measurement delay relative to the IMU (mSec)
// input noise
floatgyro_noise{1.5e-2f};///< IMU angular rate noise used for covariance prediction (rad/sec)
floataccel_noise{3.5e-1f};///< IMU acceleration noise use for covariance prediction (m/sec**2)
floatgyro_noise{1.5e-2f};///< IMU angular rate noise used for covariance prediction (rad/sec)
floataccel_noise{3.5e-1f};///< IMU acceleration noise use for covariance prediction (m/sec**2)
// process noise
floatgyro_bias_p_noise{1.0e-3f};///< process noise for IMU rate gyro bias prediction (rad/sec**2)
floataccel_bias_p_noise{1.0e-2f};///< process noise for IMU accelerometer bias prediction (m/sec**3)
floatmage_p_noise{1.0e-3f};///< process noise for earth magnetic field prediction (Gauss/sec)
floatmagb_p_noise{1.0e-4f};///< process noise for body magnetic field prediction (Gauss/sec)
floatwind_vel_p_noise{1.0e-1f};///< process noise for wind velocity prediction (m/sec**2)
constfloatwind_vel_p_noise_scaler{0.5f};///< scaling of wind process noise with vertical velocity
floatterrain_p_noise{5.0f};///< process noise for terrain offset (m/sec)
floatterrain_gradient{0.5f};///< gradient of terrain used to estimate process noise due to changing position (m/m)
constfloatterrain_timeout{10.f};///< maximum time for invalid bottom distance measurements before resetting terrain estimate (s)
floatgyro_bias_p_noise{1.0e-3f};///< process noise for IMU rate gyro bias prediction (rad/sec**2)
floataccel_bias_p_noise{1.0e-2f};///< process noise for IMU accelerometer bias prediction (m/sec**3)
floatmage_p_noise{1.0e-3f};///< process noise for earth magnetic field prediction (Gauss/sec)
floatmagb_p_noise{1.0e-4f};///< process noise for body magnetic field prediction (Gauss/sec)
floatwind_vel_p_noise{1.0e-1f};///< process noise for wind velocity prediction (m/sec**2)
constfloatwind_vel_p_noise_scaler{0.5f};///< scaling of wind process noise with vertical velocity
floatterrain_p_noise{5.0f};///< process noise for terrain offset (m/sec)
floatterrain_gradient{0.5f};///< gradient of terrain used to estimate process noise due to changing position (m/m)
constfloatterrain_timeout{10.f};///< maximum time for invalid bottom distance measurements before resetting terrain estimate (s)
// initialization errors
floatswitch_on_gyro_bias{0.1f};///< 1-sigma gyro bias uncertainty at switch on (rad/sec)
floatswitch_on_accel_bias{0.2f};///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2)
floatinitial_tilt_err{0.1f};///< 1-sigma tilt error after initial alignment using gravity vector (rad)
constfloatinitial_wind_uncertainty{1.0f};///< 1-sigma initial uncertainty in wind velocity (m/sec)
floatswitch_on_gyro_bias{0.1f};///< 1-sigma gyro bias uncertainty at switch on (rad/sec)
floatswitch_on_accel_bias{0.2f};///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2)
floatinitial_tilt_err{0.1f};///< 1-sigma tilt error after initial alignment using gravity vector (rad)
constfloatinitial_wind_uncertainty{1.0f};///< 1-sigma initial uncertainty in wind velocity (m/sec)
// position and velocity fusion
floatgps_vel_noise{5.0e-1f};///< minimum allowed observation noise for gps velocity fusion (m/sec)
floatgps_pos_noise{0.5f};///< minimum allowed observation noise for gps position fusion (m)
floatpos_noaid_noise{10.0f};///< observation noise for non-aiding position fusion (m)
floatbaro_noise{2.0f};///< observation noise for barometric height fusion (m)
floatbaro_drift_rate{0.005f};///< process noise for barometric height bias estimation (m/s)
floatbaro_innov_gate{5.0f};///< barometric and GPS height innovation consistency gate size (STD)
floatgps_pos_innov_gate{5.0f};///< GPS horizontal position innovation consistency gate size (STD)
constfloatbeta_avg_ft_us{150000.0f};///< The average time between synthetic sideslip measurements (uSec)
// range finder fusion
floatrange_noise{0.1f};///< observation noise for range finder measurements (m)
floatrange_innov_gate{5.0f};///< range finder fusion innovation consistency gate size (STD)
floatrng_gnd_clearance{0.1f};///< minimum valid value for range when on ground (m)
floatrng_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.
floatrange_noise_scaler{0.0f};///< scaling from range measurement to noise (m/m)
constfloatvehicle_variance_scaler{0.0f};///< gain applied to vehicle height variance used in calculation of height above ground observation variance
floatmax_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)
floatmax_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_trange_aid{0};///< allow switching primary height source to range finder if certain conditions are met
floatrange_aid_innov_gate{1.0f};///< gate size used for innovation consistency checks for range aid fusion
floatrange_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)
floatrange_cos_max_tilt{0.7071f};///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data
floatrange_noise{0.1f};///< observation noise for range finder measurements (m)
floatrange_innov_gate{5.0f};///< range finder fusion innovation consistency gate size (STD)
floatrng_gnd_clearance{0.1f};///< minimum valid value for range when on ground (m)
floatrng_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.
floatrange_noise_scaler{0.0f};///< scaling from range measurement to noise (m/m)
constfloatvehicle_variance_scaler{0.0f};///< gain applied to vehicle height variance used in calculation of height above ground observation variance
floatmax_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)
floatmax_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_trange_aid{0};///< allow switching primary height source to range finder if certain conditions are met
floatrange_aid_innov_gate{1.0f};///< gate size used for innovation consistency checks for range aid fusion
floatrange_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)
floatrange_cos_max_tilt{0.7071f};///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data
floatrange_kin_consistency_gate{1.0f};///< gate size used by the range finder kinematic consistency check
floatev_pos_innov_gate{5.0f};///< vision position fusion innovation consistency gate size (STD)
// optical flow fusion
floatflow_noise{0.15f};///< observation noise for optical flow LOS rate measurements (rad/sec)
floatflow_noise_qual_min{0.5f};///< observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec)
int32_tflow_qual_min{1};///< minimum acceptable quality integer from the flow sensor
floatflow_noise{0.15f};///< observation noise for optical flow LOS rate measurements (rad/sec)
floatflow_noise_qual_min{0.5f};///< observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec)
int32_tflow_qual_min{1};///< minimum acceptable quality integer from the flow sensor
floatreq_pdop{2.0f};///< maximum acceptable position dilution of precision
floatreq_hdrift{0.3f};///< maximum acceptable horizontal drift speed (m/s)
floatreq_vdrift{0.5f};///< maximum acceptable vertical drift speed (m/s)
// XYZ offset of sensors in body axes (m)
Vector3fimu_pos_body;///< xyz position of IMU in body frame (m)
Vector3fgps_pos_body;///< xyz position of the GPS antenna in body frame (m)
Vector3frng_pos_body;///< xyz position of range sensor in body frame (m)
Vector3fflow_pos_body;///< xyz position of range sensor focal point in body frame (m)
Vector3fev_pos_body;///< xyz position of VI-sensor focal point in body frame (m)
Vector3fimu_pos_body{};///< xyz position of IMU in body frame (m)
Vector3fgps_pos_body{};///< xyz position of the GPS antenna in body frame (m)
Vector3frng_pos_body{};///< xyz position of range sensor in body frame (m)
Vector3fflow_pos_body{};///< xyz position of range sensor focal point in body frame (m)
Vector3fev_pos_body{};///< xyz position of VI-sensor focal point in body frame (m)
// output complementary filter tuning
floatvel_Tau{0.25f};///< velocity state correction time constant (1/sec)
floatpos_Tau{0.25f};///< position state correction time constant (1/sec)
floatvel_Tau{0.25f};///< velocity state correction time constant (1/sec)
floatpos_Tau{0.25f};///< position state correction time constant (1/sec)
// accel bias learning control
floatacc_bias_lim{0.4f};///< maximum accel bias magnitude (m/sec**2)
floatacc_bias_learn_acc_lim{25.0f};///< learning is disabled if the magnitude of the IMU acceleration vector is greater than this (m/sec**2)
floatacc_bias_learn_gyr_lim{3.0f};///< learning is disabled if the magnitude of the IMU angular rate vector is greater than this (rad/sec)
floatacc_bias_learn_tc{0.5f};///< time constant used to control the decaying envelope filters applied to the accel and gyro magnitudes (sec)
floatacc_bias_lim{0.4f};///< maximum accel bias magnitude (m/sec**2)
floatacc_bias_learn_acc_lim{25.0f};///< learning is disabled if the magnitude of the IMU acceleration vector is greater than this (m/sec**2)
floatacc_bias_learn_gyr_lim{3.0f};///< learning is disabled if the magnitude of the IMU angular rate vector is greater than this (rad/sec)
floatacc_bias_learn_tc{0.5f};///< time constant used to control the decaying envelope filters applied to the accel and gyro magnitudes (sec)
constunsignedreset_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)
constunsignedno_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)
constunsignedreset_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)
constunsignedno_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_tvalid_timeout_max{5000000};///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec)
int32_tvalid_timeout_max{5000000};///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec)
// static barometer pressure position error coefficient along body axes
floatstatic_pressure_coef_xp{0.0f};// (-)
floatstatic_pressure_coef_xn{0.0f};// (-)
floatstatic_pressure_coef_yp{0.0f};// (-)
floatstatic_pressure_coef_yn{0.0f};// (-)
floatstatic_pressure_coef_z{0.0f};// (-)
floatstatic_pressure_coef_xp{0.0f};// (-)
floatstatic_pressure_coef_xn{0.0f};// (-)
floatstatic_pressure_coef_yp{0.0f};// (-)
floatstatic_pressure_coef_yn{0.0f};// (-)
floatstatic_pressure_coef_z{0.0f};// (-)
// upper limit on airspeed used for correction (m/s**2)
floatmax_correction_airspeed{20.0f};
// multi-rotor drag specific force fusion
floatdrag_noise{2.5f};///< observation noise variance for drag specific force measurements (m/sec**2)**2
floatbcoef_x{100.0f};///< bluff body drag ballistic coefficient for the X-axis (kg/m**2)
floatbcoef_y{100.0f};///< bluff body drag ballistic coefficient for the Y-axis (kg/m**2)
floatmcoef{0.1f};///< rotor momentum drag coefficient for the X and Y axes (1/s)
floatdrag_noise{2.5f};///< observation noise variance for drag specific force measurements (m/sec**2)**2
floatbcoef_x{100.0f};///< bluff body drag ballistic coefficient for the X-axis (kg/m**2)
floatbcoef_y{100.0f};///< bluff body drag ballistic coefficient for the Y-axis (kg/m**2)
floatmcoef{0.1f};///< rotor momentum drag coefficient for the X and Y axes (1/s)
// control of accel error detection and mitigation (IMU clipping)
constfloatvert_innov_test_lim{3.0f};///< Number of standard deviations of vertical vel/pos innovations allowed before triggering a vertical acceleration failure
constfloatvert_innov_test_min{1.0f};///< Minimum number of standard deviations of vertical vel/pos innovations required to trigger a vertical acceleration failure
constintbad_acc_reset_delay_us{500000};///< Continuous time that the vertical position and velocity innovation test must fail before the states are reset (uSec)
constfloatvert_innov_test_lim{3.0f};///< Number of standard deviations of vertical vel/pos innovations allowed before triggering a vertical acceleration failure
constfloatvert_innov_test_min{1.0f};///< Minimum number of standard deviations of vertical vel/pos innovations required to trigger a vertical acceleration failure
constintbad_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
constfloatauxvel_noise{0.5f};///< minimum observation noise, uses reported noise if greater (m/s)
// compute synthetic magnetomter Z value if possible
int32_tsynthesize_mag_z{0};
int32_tcheck_mag_strength{0};
// Parameters used to control when yaw is reset to the EKF-GSF yaw estimator value
floatEKFGSF_tas_default{15.0f};///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s)
constunsignedEKFGSF_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
constfloatEKFGSF_yaw_err_max{0.262f};///< Composite yaw 1-sigma uncertainty threshold used to check for convergence (rad)
constunsignedEKFGSF_reset_count_limit{3};///< Maximum number of times the yaw can be reset to the EKF-GSF yaw estimator value
floatEKFGSF_tas_default{15.0f};///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s)
constunsignedEKFGSF_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
constfloatEKFGSF_yaw_err_max{0.262f};///< Composite yaw 1-sigma uncertainty threshold used to check for convergence (rad)
constunsignedEKFGSF_reset_count_limit{3};///< Maximum number of times the yaw can be reset to the EKF-GSF yaw estimator value
};
structstateSample{
Quatfquat_nominal;///< quaternion defining the rotation from body to earth frame
Vector3fvel;///< NED velocity in earth frame in m/s
Vector3fpos;///< NED position in earth frame in m
Vector3fdelta_ang_bias;///< delta angle bias estimate in rad
Vector3fdelta_vel_bias;///< delta velocity bias estimate in m/s
Vector3fmag_I;///< NED earth magnetic field in gauss
Vector3fmag_B;///< magnetometer bias estimate in body frame in gauss
Vector2fwind_vel;///< horizontal wind velocity in earth frame in m/s
Quatfquat_nominal{};///< quaternion defining the rotation from body to earth frame
Vector3fvel{};///< NED velocity in earth frame in m/s
Vector3fpos{};///< NED position in earth frame in m
Vector3fdelta_ang_bias{};///< delta angle bias estimate in rad
Vector3fdelta_vel_bias{};///< delta velocity bias estimate in m/s
Vector3fmag_I{};///< NED earth magnetic field in gauss
Vector3fmag_B{};///< magnetometer bias estimate in body frame in gauss
Vector2fwind_vel{};///< horizontal wind velocity in earth frame in m/s
};
unionfault_status_u{
struct{
boolbad_mag_x:1;///< 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error
boolbad_mag_y:1;///< 1 - true if the fusion of the magnetometer Y-axis has encountered a numerical error
boolbad_mag_z:1;///< 2 - true if the fusion of the magnetometer Z-axis has encountered a numerical error
boolbad_hdg:1;///< 3 - true if the fusion of the heading angle has encountered a numerical error
boolbad_mag_decl:1;///< 4 - true if the fusion of the magnetic declination has encountered a numerical error
boolbad_airspeed:1;///< 5 - true if fusion of the airspeed has encountered a numerical error
boolbad_sideslip:1;///< 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error
boolbad_optflow_X:1;///< 7 - true if fusion of the optical flow X axis has encountered a numerical error
boolbad_optflow_Y:1;///< 8 - true if fusion of the optical flow Y axis has encountered a numerical error
boolbad_vel_N:1;///< 9 - true if fusion of the North velocity has encountered a numerical error
boolbad_vel_E:1;///< 10 - true if fusion of the East velocity has encountered a numerical error
boolbad_vel_D:1;///< 11 - true if fusion of the Down velocity has encountered a numerical error
boolbad_pos_N:1;///< 12 - true if fusion of the North position has encountered a numerical error
boolbad_pos_E:1;///< 13 - true if fusion of the East position has encountered a numerical error
boolbad_pos_D:1;///< 14 - true if fusion of the Down position has encountered a numerical error
boolbad_acc_bias:1;///< 15 - true if bad delta velocity bias estimates have been detected
boolbad_acc_vertical:1;///< 16 - true if bad vertical accelerometer data has been detected
boolbad_acc_clipping:1;///< 17 - true if delta velocity data contains clipping (asymmetric railing)
boolbad_mag_x:1;///< 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error
boolbad_mag_y:1;///< 1 - true if the fusion of the magnetometer Y-axis has encountered a numerical error
boolbad_mag_z:1;///< 2 - true if the fusion of the magnetometer Z-axis has encountered a numerical error
boolbad_hdg:1;///< 3 - true if the fusion of the heading angle has encountered a numerical error
boolbad_mag_decl:1;///< 4 - true if the fusion of the magnetic declination has encountered a numerical error
boolbad_airspeed:1;///< 5 - true if fusion of the airspeed has encountered a numerical error
boolbad_sideslip:1;///< 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error
boolbad_optflow_X:1;///< 7 - true if fusion of the optical flow X axis has encountered a numerical error
boolbad_optflow_Y:1;///< 8 - true if fusion of the optical flow Y axis has encountered a numerical error
boolbad_vel_N:1;///< 9 - true if fusion of the North velocity has encountered a numerical error
boolbad_vel_E:1;///< 10 - true if fusion of the East velocity has encountered a numerical error
boolbad_vel_D:1;///< 11 - true if fusion of the Down velocity has encountered a numerical error
boolbad_pos_N:1;///< 12 - true if fusion of the North position has encountered a numerical error
boolbad_pos_E:1;///< 13 - true if fusion of the East position has encountered a numerical error
boolbad_pos_D:1;///< 14 - true if fusion of the Down position has encountered a numerical error
boolbad_acc_bias:1;///< 15 - true if bad delta velocity bias estimates have been detected
boolbad_acc_vertical:1;///< 16 - true if bad vertical accelerometer data has been detected
boolbad_acc_clipping:1;///< 17 - true if delta velocity data contains clipping (asymmetric railing)
}flags;
uint32_tvalue;
};
// define structure used to communicate innovation test failures
unioninnovation_fault_status_u{
struct{
boolreject_hor_vel:1;///< 0 - true if horizontal velocity observations have been rejected
boolreject_ver_vel:1;///< 1 - true if vertical velocity observations have been rejected
boolreject_hor_pos:1;///< 2 - true if horizontal position observations have been rejected
boolreject_ver_pos:1;///< 3 - true if true if vertical position observations have been rejected
boolreject_mag_x:1;///< 4 - true if the X magnetometer observation has been rejected
boolreject_mag_y:1;///< 5 - true if the Y magnetometer observation has been rejected
boolreject_mag_z:1;///< 6 - true if the Z magnetometer observation has been rejected
boolreject_yaw:1;///< 7 - true if the yaw observation has been rejected
boolreject_airspeed:1;///< 8 - true if the airspeed observation has been rejected
boolreject_sideslip:1;///< 9 - true if the synthetic sideslip observation has been rejected
boolreject_hagl:1;///< 10 - true if the height above ground observation has been rejected
boolreject_optflow_X:1;///< 11 - true if the X optical flow observation has been rejected
boolreject_optflow_Y:1;///< 12 - true if the Y optical flow observation has been rejected
boolreject_hor_vel:1;///< 0 - true if horizontal velocity observations have been rejected
boolreject_ver_vel:1;///< 1 - true if vertical velocity observations have been rejected
boolreject_hor_pos:1;///< 2 - true if horizontal position observations have been rejected
boolreject_ver_pos:1;///< 3 - true if true if vertical position observations have been rejected
boolreject_mag_x:1;///< 4 - true if the X magnetometer observation has been rejected
boolreject_mag_y:1;///< 5 - true if the Y magnetometer observation has been rejected
boolreject_mag_z:1;///< 6 - true if the Z magnetometer observation has been rejected
boolreject_yaw:1;///< 7 - true if the yaw observation has been rejected
boolreject_airspeed:1;///< 8 - true if the airspeed observation has been rejected
boolreject_sideslip:1;///< 9 - true if the synthetic sideslip observation has been rejected
boolreject_hagl:1;///< 10 - true if the height above ground observation has been rejected
boolreject_optflow_X:1;///< 11 - true if the X optical flow observation has been rejected
boolreject_optflow_Y:1;///< 12 - true if the Y optical flow observation has been rejected
}flags;
uint16_tvalue;
};
// publish the status of various GPS quality checks
@ -464,35 +464,35 @@ union gps_check_fail_status_u {
@@ -464,35 +464,35 @@ union gps_check_fail_status_u {
// bitmask containing filter control status
unionfilter_control_status_u{
struct{
uint32_ttilt_align:1;///< 0 - true if the filter tilt alignment is complete
uint32_tyaw_align:1;///< 1 - true if the filter yaw alignment is complete
uint32_tgps:1;///< 2 - true if GPS measurement fusion is intended
uint32_topt_flow:1;///< 3 - true if optical flow measurements fusion is intended
uint32_tmag_hdg:1;///< 4 - true if a simple magnetic yaw heading fusion is intended
uint32_tmag_3D:1;///< 5 - true if 3-axis magnetometer measurement fusion is inteded
uint32_tmag_dec:1;///< 6 - true if synthetic magnetic declination measurements fusion is intended
uint32_tin_air:1;///< 7 - true when the vehicle is airborne
uint32_twind:1;///< 8 - true when wind velocity is being estimated
uint32_tbaro_hgt:1;///< 9 - true when baro height is being fused as a primary height reference
uint32_trng_hgt:1;///< 10 - true when range finder height is being fused as a primary height reference
uint32_tgps_hgt:1;///< 11 - true when GPS height is being fused as a primary height reference
uint32_tev_pos:1;///< 12 - true when local position data fusion from external vision is intended
uint32_tev_yaw:1;///< 13 - true when yaw data from external vision measurements fusion is intended
uint32_tev_hgt:1;///< 14 - true when height data from external vision measurements is being fused
uint32_tfuse_beta:1;///< 15 - true when synthetic sideslip measurements are being fused
uint32_tmag_field_disturbed:1;///< 16 - true when the mag field does not match the expected strength
uint32_tfixed_wing:1;///< 17 - true when the vehicle is operating as a fixed wing vehicle
uint32_tmag_fault:1;///< 18 - true when the magnetometer has been declared faulty and is no longer being used
uint32_tfuse_aspd:1;///< 19 - true when airspeed measurements are being fused
uint32_tgnd_effect:1;///< 20 - true when protection from ground effect induced static pressure rise is active
uint32_trng_stuck:1;///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough
uint32_tgps_yaw:1;///< 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended
uint32_ttilt_align:1;///< 0 - true if the filter tilt alignment is complete
uint32_tyaw_align:1;///< 1 - true if the filter yaw alignment is complete
uint32_tgps:1;///< 2 - true if GPS measurement fusion is intended
uint32_topt_flow:1;///< 3 - true if optical flow measurements fusion is intended
uint32_tmag_hdg:1;///< 4 - true if a simple magnetic yaw heading fusion is intended
uint32_tmag_3D:1;///< 5 - true if 3-axis magnetometer measurement fusion is inteded
uint32_tmag_dec:1;///< 6 - true if synthetic magnetic declination measurements fusion is intended
uint32_tin_air:1;///< 7 - true when the vehicle is airborne
uint32_twind:1;///< 8 - true when wind velocity is being estimated
uint32_tbaro_hgt:1;///< 9 - true when baro height is being fused as a primary height reference
uint32_trng_hgt:1;///< 10 - true when range finder height is being fused as a primary height reference
uint32_tgps_hgt:1;///< 11 - true when GPS height is being fused as a primary height reference
uint32_tev_pos:1;///< 12 - true when local position data fusion from external vision is intended
uint32_tev_yaw:1;///< 13 - true when yaw data from external vision measurements fusion is intended
uint32_tev_hgt:1;///< 14 - true when height data from external vision measurements is being fused
uint32_tfuse_beta:1;///< 15 - true when synthetic sideslip measurements are being fused
uint32_tmag_field_disturbed:1;///< 16 - true when the mag field does not match the expected strength
uint32_tfixed_wing:1;///< 17 - true when the vehicle is operating as a fixed wing vehicle
uint32_tmag_fault:1;///< 18 - true when the magnetometer has been declared faulty and is no longer being used
uint32_tfuse_aspd:1;///< 19 - true when airspeed measurements are being fused
uint32_tgnd_effect:1;///< 20 - true when protection from ground effect induced static pressure rise is active
uint32_trng_stuck:1;///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough
uint32_tgps_yaw:1;///< 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended
uint32_tmag_aligned_in_flight:1;///< 23 - true when the in-flight mag field alignment has been completed
uint32_tev_vel:1;///< 24 - true when local frame velocity data fusion from external vision measurements is intended
uint32_tsynthetic_mag_z:1;///< 25 - true when we are using a synthesized measurement for the magnetometer Z component
uint32_tvehicle_at_rest:1;///< 26 - true when the vehicle is at rest
uint32_tgps_yaw_fault:1;///< 27 - true when the GNSS heading has been declared faulty and is no longer being used
uint32_trng_fault:1;///< 28 - true when the range finder has been declared faulty and is no longer being used
uint32_tev_vel:1;///< 24 - true when local frame velocity data fusion from external vision measurements is intended
uint32_tsynthetic_mag_z:1;///< 25 - true when we are using a synthesized measurement for the magnetometer Z component
uint32_tvehicle_at_rest:1;///< 26 - true when the vehicle is at rest
uint32_tgps_yaw_fault:1;///< 27 - true when the GNSS heading has been declared faulty and is no longer being used
uint32_trng_fault:1;///< 28 - true when the range finder has been declared faulty and is no longer being used
uint32_tinertial_dead_reckoning:1;///< 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift
uint32_twind_dead_reckoning:1;///< 30 - true if we are navigationg reliant on wind relative measurements
uint32_trng_kin_consistent:1;///< 31 - true when the range finder kinematic consistency check is passing
@ -521,8 +521,8 @@ union ekf_solution_status {
@@ -521,8 +521,8 @@ union ekf_solution_status {
unionterrain_fusion_status_u{
struct{
boolrange_finder:1;///< 0 - true if we are fusing range finder data
boolflow:1;///< 1 - true if we are fusing flow data
boolrange_finder:1;///< 0 - true if we are fusing range finder data
boolflow:1;///< 1 - true if we are fusing flow data
}flags;
uint8_tvalue;
};
@ -530,19 +530,19 @@ union terrain_fusion_status_u {
@@ -530,19 +530,19 @@ union terrain_fusion_status_u {
// define structure used to communicate information events
unioninformation_event_status_u{
struct{
boolgps_checks_passed:1;///< 0 - true when gps quality checks are passing passed
boolreset_vel_to_gps:1;///< 1 - true when the velocity states are reset to the gps measurement
boolreset_vel_to_flow:1;///< 2 - true when the velocity states are reset using the optical flow measurement
boolreset_vel_to_vision:1;///< 3 - true when the velocity states are reset to the vision system measurement
boolreset_vel_to_zero:1;///< 4 - true when the velocity states are reset to zero
boolreset_pos_to_last_known:1;///< 5 - true when the position states are reset to the last known position
boolreset_pos_to_gps:1;///< 6 - true when the position states are reset to the gps measurement
boolreset_pos_to_vision:1;///< 7 - true when the position states are reset to the vision system measurement
boolstarting_gps_fusion:1;///< 8 - true when the filter starts using gps measurements to correct the state estimates
boolstarting_vision_pos_fusion:1;///< 9 - true when the filter starts using vision system position measurements to correct the state estimates
boolstarting_vision_vel_fusion:1;///< 10 - true when the filter starts using vision system velocity measurements to correct the state estimates
boolstarting_vision_yaw_fusion:1;///< 11 - true when the filter starts using vision system yaw measurements to correct the state estimates
boolyaw_aligned_to_imu_gps:1;///< 12 - true when the filter resets the yaw to an estimate derived from IMU and GPS data
boolgps_checks_passed:1;///< 0 - true when gps quality checks are passing passed
boolreset_vel_to_gps:1;///< 1 - true when the velocity states are reset to the gps measurement
boolreset_vel_to_flow:1;///< 2 - true when the velocity states are reset using the optical flow measurement
boolreset_vel_to_vision:1;///< 3 - true when the velocity states are reset to the vision system measurement
boolreset_vel_to_zero:1;///< 4 - true when the velocity states are reset to zero
boolreset_pos_to_last_known:1;///< 5 - true when the position states are reset to the last known position
boolreset_pos_to_gps:1;///< 6 - true when the position states are reset to the gps measurement
boolreset_pos_to_vision:1;///< 7 - true when the position states are reset to the vision system measurement
boolstarting_gps_fusion:1;///< 8 - true when the filter starts using gps measurements to correct the state estimates
boolstarting_vision_pos_fusion:1;///< 9 - true when the filter starts using vision system position measurements to correct the state estimates
boolstarting_vision_vel_fusion:1;///< 10 - true when the filter starts using vision system velocity measurements to correct the state estimates
boolstarting_vision_yaw_fusion:1;///< 11 - true when the filter starts using vision system yaw measurements to correct the state estimates
boolyaw_aligned_to_imu_gps:1;///< 12 - true when the filter resets the yaw to an estimate derived from IMU and GPS data
}flags;
uint32_tvalue;
};
@ -550,17 +550,17 @@ union information_event_status_u {
@@ -550,17 +550,17 @@ union information_event_status_u {
// define structure used to communicate information events
unionwarning_event_status_u{
struct{
boolgps_quality_poor:1;///< 0 - true when the gps is failing quality checks
boolgps_fusion_timout:1;///< 1 - true when the gps data has not been used to correct the state estimates for a significant time period
boolgps_data_stopped:1;///< 2 - true when the gps data has stopped for a significant time period
boolgps_data_stopped_using_alternate:1;///< 3 - true when the gps data has stopped for a significant time period but the filter is able to use other sources of data to maintain navigation
boolheight_sensor_timeout:1;///< 4 - true when the height sensor has not been used to correct the state estimates for a significant time period
boolstopping_navigation:1;///< 5 - true when the filter has insufficient data to estimate velocity and position and is falling back to an attitude, height and height rate mode of operation
boolinvalid_accel_bias_cov_reset:1;///< 6 - true when the filter has detected bad acceerometer bias state estimates and has reset the corresponding covariance matrix elements
boolbad_yaw_using_gps_course:1;///< 7 - true when the fiter has detected an invalid yaw esitmate and has reset the yaw angle to the GPS ground course
boolstopping_mag_use:1;///< 8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetomer data
boolvision_data_stopped:1;///< 9 - true when the vision system data has stopped for a significant time period
boolemergency_yaw_reset_mag_stopped:1;///< 10 - true when the filter has detected bad magnetometer data, has reset the yaw to anothter source of data and has stopped further use of the magnetomer data
boolgps_quality_poor:1;///< 0 - true when the gps is failing quality checks
boolgps_fusion_timout:1;///< 1 - true when the gps data has not been used to correct the state estimates for a significant time period
boolgps_data_stopped:1;///< 2 - true when the gps data has stopped for a significant time period
boolgps_data_stopped_using_alternate:1;///< 3 - true when the gps data has stopped for a significant time period but the filter is able to use other sources of data to maintain navigation
boolheight_sensor_timeout:1;///< 4 - true when the height sensor has not been used to correct the state estimates for a significant time period
boolstopping_navigation:1;///< 5 - true when the filter has insufficient data to estimate velocity and position and is falling back to an attitude, height and height rate mode of operation
boolinvalid_accel_bias_cov_reset:1;///< 6 - true when the filter has detected bad acceerometer bias state estimates and has reset the corresponding covariance matrix elements
boolbad_yaw_using_gps_course:1;///< 7 - true when the fiter has detected an invalid yaw esitmate and has reset the yaw angle to the GPS ground course
boolstopping_mag_use:1;///< 8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetomer data
boolvision_data_stopped:1;///< 9 - true when the vision system data has stopped for a significant time period
boolemergency_yaw_reset_mag_stopped:1;///< 10 - true when the filter has detected bad magnetometer data, has reset the yaw to anothter source of data and has stopped further use of the magnetomer data
boolemergency_yaw_reset_gps_yaw_stopped:1;///< 11 - true when the filter has detected bad GNSS yaw data, has reset the yaw to anothter source of data and has stopped further use of the GNSS yaw data
mcsauder
3 years ago
committed by
Daniel Agar