178 #define VDIST_SENSOR_BARO 0 179 #define VDIST_SENSOR_GPS 1
180 #define VDIST_SENSOR_RANGE 2
181 #define VDIST_SENSOR_EV 3
184 #define MASK_USE_GEO_DECL (1<<0) 185 #define MASK_SAVE_GEO_DECL (1<<1)
186 #define MASK_FUSE_DECL (1<<2)
189 #define MASK_USE_GPS (1<<0) 190 #define MASK_USE_OF (1<<1)
191 #define MASK_INHIBIT_ACC_BIAS (1<<2)
192 #define MASK_USE_EVPOS (1<<3)
193 #define MASK_USE_EVYAW (1<<4)
194 #define MASK_USE_DRAG (1<<5)
195 #define MASK_ROTATE_EV (1<<6)
196 #define MASK_USE_GPSYAW (1<<7)
197 #define MASK_USE_EVVEL (1<<8)
200 #define MAG_FUSE_TYPE_AUTO 0 201 #define MAG_FUSE_TYPE_HEADING 1
202 #define MAG_FUSE_TYPE_3D 2
203 #define MAG_FUSE_TYPE_UNUSED 3
204 #define MAG_FUSE_TYPE_INDOOR 4
205 #define MAG_FUSE_TYPE_NONE 5
208 #define GPS_MAX_INTERVAL (uint64_t)5e5 209 #define BARO_MAX_INTERVAL (uint64_t)2e5
210 #define RNG_MAX_INTERVAL (uint64_t)2e5
211 #define EV_MAX_INTERVAL (uint64_t)2e5
214 #define BADACC_PROBATION (uint64_t)10e6 215 #define BADACC_BIAS_PNOISE 4.9f
218 #define GNDEFFECT_TIMEOUT 10E6 224 int32_t sensor_interval_min_ms{20};
227 float min_delay_ms{0.0f};
228 float mag_delay_ms{0.0f};
229 float baro_delay_ms{0.0f};
230 float gps_delay_ms{110.0f};
231 float airspeed_delay_ms{100.0f};
232 float flow_delay_ms{5.0f};
233 float range_delay_ms{5.0f};
234 float ev_delay_ms{100.0f};
235 float auxvel_delay_ms{0.0f};
238 float gyro_noise{1.5e-2
f};
239 float accel_noise{3.5e-1
f};
242 float gyro_bias_p_noise{1.0e-3
f};
243 float accel_bias_p_noise{6.0e-3
f};
244 float mage_p_noise{1.0e-3
f};
245 float magb_p_noise{1.0e-4
f};
246 float wind_vel_p_noise{1.0e-1
f};
247 float wind_vel_p_noise_scaler{0.5f};
248 float terrain_p_noise{5.0f};
249 float terrain_gradient{0.5f};
252 float switch_on_gyro_bias{0.1f};
253 float switch_on_accel_bias{0.2f};
254 float initial_tilt_err{0.1f};
255 float initial_wind_uncertainty{1.0f};
258 float gps_vel_noise{5.0e-1
f};
259 float gps_pos_noise{0.5f};
260 float pos_noaid_noise{10.0f};
261 float baro_noise{2.0f};
262 float baro_innov_gate{5.0f};
263 float gps_pos_innov_gate{5.0f};
264 float gps_vel_innov_gate{5.0f};
265 float gnd_effect_deadzone{5.0f};
266 float gnd_effect_max_hgt{0.5f};
269 float mag_heading_noise{3.0e-1
f};
270 float mag_noise{5.0e-2
f};
271 float mag_declination_deg{0.0f};
272 float heading_innov_gate{2.6f};
273 float mag_innov_gate{3.0f};
274 int32_t mag_declination_source{7};
275 int32_t mag_fusion_type{0};
276 float mag_acc_gate{0.5f};
277 float mag_yaw_rate_gate{0.25f};
280 float tas_innov_gate{5.0f};
281 float eas_noise{1.4f};
284 float beta_innov_gate{5.0f};
285 float beta_noise{0.3f};
286 float beta_avg_ft_us{150000.0f};
289 float range_noise{0.1f};
290 float range_innov_gate{5.0f};
291 float rng_gnd_clearance{0.1f};
292 float rng_sens_pitch{0.0f};
293 float range_noise_scaler{0.0f};
294 float vehicle_variance_scaler{0.0f};
295 float max_hagl_for_range_aid{5.0f};
296 float max_vel_for_range_aid{1.0f};
297 int32_t range_aid{0};
298 float range_aid_innov_gate{1.0f};
299 float range_cos_max_tilt{0.7071f};
300 float range_stuck_threshold{0.1f};
301 int32_t range_signal_hysteresis_ms{1000};
304 float ev_vel_innov_gate{3.0f};
305 float ev_pos_innov_gate{5.0f};
308 float flow_noise{0.15f};
309 float flow_noise_qual_min{0.5f};
310 int32_t flow_qual_min{1};
311 float flow_innov_gate{3.0f};
315 int32_t gps_check_mask{21};
316 float req_hacc{5.0f};
317 float req_vacc{8.0f};
318 float req_sacc{1.0f};
319 int32_t req_nsats{6};
320 float req_pdop{2.0f};
321 float req_hdrift{0.3f};
322 float req_vdrift{0.5f};
332 float vel_Tau{0.25f};
333 float pos_Tau{0.25f};
336 float acc_bias_lim{0.4f};
337 float acc_bias_learn_acc_lim{25.0f};
338 float acc_bias_learn_gyr_lim{3.0f};
339 float acc_bias_learn_tc{0.5f};
341 unsigned reset_timeout_max{7000000};
342 unsigned no_aid_timeout_max{1000000};
344 int32_t valid_timeout_max{5000000};
347 float drag_noise{2.5f};
348 float bcoef_x{25.0f};
349 float bcoef_y{25.0f};
352 float vert_innov_test_lim{4.5f};
353 int bad_acc_reset_delay_us{500000};
356 float auxvel_noise{0.5f};
357 float auxvel_gate{5.0f};
360 float is_moving_scaler{1.0f};
363 int32_t synthesize_mag_z{0};
364 int32_t check_mag_strength{0};
float eph
GPS horizontal position accuracy in m.
uint16_t velocity_horiz
1 - True if the horizontal velocity estimate is good
Vector3f delta_ang
delta angle in body frame (integrated gyro measurements) (rad)
float yaw_offset
Heading/Yaw offset for dual antenna GPS - refer to description for GPS_YAW_OFFSET.
float hgt
gps height measurement (m)
uint32_t gps_yaw
22 - true when yaw (not ground course) data from a GPS receiver is being fused
bool reject_vel_NED
0 - true if velocity observations have been rejected
float epv
GPS vertical position accuracy in m.
bool reject_pos_NE
1 - true if horizontal position observations have been rejected
uint32_t yaw_align
1 - true if the filter yaw alignment is complete
uint8_t nsats
number of satellites used
bool bad_mag_x
0 - true if the fusion of the magnetometer X-axis has encountered a numerical error ...
float vel_m_s
GPS ground speed (m/sec)
bool bad_mag_y
1 - true if the fusion of the magnetometer Y-axis has encountered a numerical error ...
bool bad_optflow_X
7 - true if fusion of the optical flow X axis has encountered a numerical error
bool bad_pos_D
14 - true if fusion of the Down position has encountered a numerical error
Vector3f ev_pos_body
xyz position of VI-sensor focal point in body frame (m)
bool reject_pos_D
2 - true if true if vertical position observations have been rejected
uint16_t pos_horiz_abs
4 - True if the horizontal position (absolute) estimate is good
Vector3f accel_bias
delta velocity bias estimate in m/s
uint16_t nsats
1 - true if number of satellites used is insufficient
bool bad_mag_decl
4 - true if the fusion of the magnetic declination has encountered a numerical error ...
uint32_t fuse_aspd
19 - true when airspeed measurements are being fused
Vector2f pos
NE earth frame gps horizontal position measurement (m)
uint32_t fixed_wing
17 - true when the vehicle is operating as a fixed wing vehicle
bool reject_mag_x
3 - true if the X magnetometer observation has been rejected
Quatf quat
quaternion defining rotation from body to earth frame
uint32_t mag_hdg
4 - true if a simple magnetic yaw heading is being fused
SquareMatrix< float, 3 > Matrix3f
uint16_t pred_pos_horiz_rel
8 - True if the EKF has sufficient data to enter a mode that will provide a (relative) position estim...
uint64_t time_us
timestamp of the measurement (uSec)
uint32_t mag_fault
18 - true when the magnetometer has been declared faulty and is no longer being used ...
float sacc
GPS speed accuracy in m/s.
bool reject_hagl
9 - true if the height above ground observation has been rejected
Vector2f accelXY
measured specific force along the X and Y body axes (m/sec**2)
Vector3f gyrodata
Gyro rates about the XYZ body axes (rad/sec)
uint16_t hdrift
6 - true if horizontal drift is excessive (can only be used when stationary on ground) ...
uint32_t rng_hgt
10 - true when range finder height is being fused as a primary height reference
bool bad_vel_N
9 - true if fusion of the North velocity has encountered a numerical error
uint64_t time_us
timestamp of the measurement (uSec)
uint16_t pos_vert_abs
5 - True if the vertical position (absolute) estimate is good
uint8_t fix_type
0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic ...
Vector3f mag_I
NED earth magnetic field in gauss.
uint32_t mag_field_disturbed
16 - true when the mag field does not match the expected strength
bool reject_mag_z
5 - true if the Z magnetometer observation has been rejected
bool bad_pos_N
12 - true if fusion of the North position has encountered a numerical error
uint32_t baro_hgt
9 - true when baro height is being fused as a primary height reference
float pdop
position dilution of precision
uint16_t sacc
5 - true if reported speed accuracy is insufficient
float vacc
1-std vertical position error (m)
uint16_t vacc
4 - true if reported vertical accuracy is insufficient
uint16_t velocity_vert
2 - True if the vertical velocity estimate is good
Vector3f gyroXYZ
measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (...
float vel_ned[3]
GPS ground speed NED.
uint32_t dt
integration time of flow samples (sec)
float hgtErr
1-Sigma height accuracy (m)
float dt
amount of integration time (sec)
uint16_t vspeed
9 - true if vertical speed error is excessive
Vector3f pos
XYZ position in earth frame (m) - Z must be aligned with down axis.
uint8_t quality
Quality of Flow data.
float vel_d
D velocity calculated using alternative algorithm (m/sec)
float rng
range (distance to ground) measurement (m)
Vector2f velVarNE
estimated error variance of the NE velocity (m/sec)**2
float yaw
yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
float sacc
1-std speed error (m/sec)
int8_t quality
Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality.
uint32_t synthetic_mag_z
25 - true when we are using a synthesized measurement for the magnetometer Z component ...
float velErr
1-Sigma velocity accuracy (m/sec)
bool bad_mag_z
2 - true if the fusion of the magnetometer Z-axis has encountered a numerical error ...
uint64_t time_us
timestamp of the measurement (uSec)
bool bad_acc_bias
15 - true if bad delta velocity bias estimates have been detected
uint32_t ev_vel
24 - true when local earth frame velocity data from external vision measurements are being fused ...
Vector3f rng_pos_body
xyz position of range sensor in body frame (m)
uint16_t pdop
2 - true if position dilution of precision is insufficient
uint64_t time_us
timestamp of the measurement (uSec)
uint16_t vdrift
7 - true if vertical drift is excessive (can only be used when stationary on ground) ...
float hgtErr
1-Sigma height accuracy (m)
uint64_t time_us
timestamp of the measurement (uSec)
float posErr
1-Sigma horizontal position accuracy (m)
uint32_t gnd_effect
20 - true when protection from ground effect induced static pressure rise is active ...
bool vel_ned_valid
GPS ground speed is valid.
Vector3f vel
NED earth frame gps velocity measurement (m/sec)
float eas2tas
equivalent to true airspeed factor
bool bad_airspeed
5 - true if fusion of the airspeed has encountered a numerical error
Vector3f flow_pos_body
xyz position of range sensor focal point in body frame (m)
uint64_t time_us
timestamp of the measurement (uSec)
uint16_t const_pos_mode
7 - True if the EKF is in a constant position mode and is not using external measurements (eg GPS or ...
Vector3f pos
NED position estimate in earth frame (m/sec)
Vector3f gps_pos_body
xyz position of the GPS antenna in body frame (m)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
bool bad_hdg
3 - true if the fusion of the heading angle has encountered a numerical error
uint16_t attitude
0 - True if the attitude estimate is good
Vector2< float > Vector2f
uint64_t time_us
timestamp of the measurement (uSec)
uint16_t hspeed
8 - true if horizontal speed is excessive (can only be used when stationary on ground) ...
uint8_t quality
quality indicator between 0 and 255
Vector3f pos
XYZ position in earth frame (m) - Z must be aligned with down axis.
Vector3f vel
NED velocity in earth frame in m/s.
int32_t lat
Latitude in 1E-7 degrees.
uint16_t pos_horiz_rel
3 - True if the horizontal position (relative) estimate is good
uint32_t ev_hgt
14 - true when height data from external vision measurements is being fused
Vector2f flowRadXY
measured delta angle of the image about the X and Y body axes (rad), RH rotation is positive ...
uint32_t ev_yaw
13 - true when yaw data from external vision measurements is being fused
uint16_t pos_vert_agl
6 - True if the vertical position (above ground) estimate is good
bool reject_yaw
6 - true if the yaw observation has been rejected
bool reject_optflow_Y
11 - true if the Y optical flow observation has been rejected
Vector2f velNE
measured NE velocity relative to the local origin (m/sec)
uint32_t mag_aligned_in_flight
23 - true when the in-flight mag field alignment has been completed
Vector2f flowdata
Optical flow rates about the X and Y body axes (rad/sec)
#define MASK_USE_GPS
set to true to use GPS data
Type wrap_pi(Type x)
Wrap value in range [-π, π)
Quatf quat_nominal
quaternion defining the rotation from body to earth frame
Vector3f pos
NED position in earth frame in m.
Vector3f vel
XYZ velocity in earth frame (m/sec) - Z must be aligned with down axis.
uint32_t mag_dec
6 - true if synthetic magnetic declination measurements are being fused
Vector3< float > Vector3f
float yaw
yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
uint32_t opt_flow
3 - true if optical flow measurements are being fused
float true_airspeed
true airspeed measurement (m/sec)
#define VDIST_SENSOR_BARO
Use baro height.
Vector3f mag_B
magnetometer bias estimate in body frame in gauss
uint16_t fix
0 - true if the fix type is insufficient (no 3D solution)
bool bad_vel_E
10 - true if fusion of the East velocity has encountered a numerical error
bool reject_optflow_X
10 - true if the X optical flow observation has been rejected
bool bad_sideslip
6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error ...
uint32_t mag_3D
5 - true if 3-axis magnetometer measurement are being fused
Quaternion< float > Quatf
uint64_t time_us
timestamp of the measurement (uSec)
float angErr
1-Sigma angular error (rad)
Vector3f vel
NED velocity estimate in earth frame (m/sec)
bool reject_sideslip
8 - true if the synthetic sideslip observation has been rejected
uint32_t in_air
7 - true when the vehicle is airborne
uint16_t hacc
3 - true if reported horizontal accuracy is insufficient
float vel_d_integ
Integral of vel_d (m)
bool reject_mag_y
4 - true if the Y magnetometer observation has been rejected
int32_t alt
Altitude in 1E-3 meters (millimeters) above MSL.
uint16_t pred_pos_horiz_abs
9 - True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estim...
uint32_t wind
8 - true when wind velocity is being estimated
Quatf quat
quaternion defining rotation from body to earth frame
uint32_t ev_pos
12 - true when local position data from external vision is being fused
Vector3f imu_pos_body
xyz position of IMU in body frame (m)
uint32_t fuse_beta
15 - true when synthetic sideslip measurements are being fused
float hacc
1-std horizontal position error (m)
AxisAngle< float > AxisAnglef
Vector3f vel
XYZ velocity in earth frame (m/sec) - Z must be aligned with down axis.
uint32_t gps_hgt
11 - true when GPS height is being fused as a primary height reference
float angErr
1-Sigma angular error (rad)
uint32_t rng_stuck
21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough ...
bool bad_optflow_Y
8 - true if fusion of the optical flow Y axis has encountered a numerical error
uint32_t gps
2 - true if GPS measurements are being fused
uint32_t tilt_align
0 - true if the filter tilt alignment is complete
Vector2f wind_vel
wind velocity in m/s
float velErr
1-Sigma velocity accuracy (m/sec)
Vector3f mag
NED magnetometer body frame measurements (Gauss)
bool reject_airspeed
7 - true if the airspeed observation has been rejected
Vector3f delta_vel
delta velocity in body frame (integrated accelerometer measurements) (m/sec)
Vector3f gyro_bias
delta angle bias estimate in rad
Quatf quat_nominal
nominal quaternion describing vehicle attitude
float delta_ang_dt
delta angle integration period (sec)
uint64_t time_us
timestamp of the integration period leading edge (uSec)
uint64_t time_us
timestamp of the measurement (uSec)
uint64_t time_us
timestamp of the measurement (uSec)
uint16_t accel_error
11 - True if the EKF has detected bad accelerometer data
float delta_vel_dt
delta velocity integration period (sec)
bool bad_vel_D
11 - true if fusion of the Down velocity has encountered a numerical error
bool bad_pos_E
13 - true if fusion of the East position has encountered a numerical error
int32_t lon
Longitude in 1E-7 degrees.
float posErr
1-Sigma horizontal position accuracy (m)
uint16_t gps_glitch
10 - True if the EKF has detected a GPS glitch