PX4 Firmware
PX4 Autopilot Software http://px4.io
common.h
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name ECL nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file common.h
36  * Definition of base class for attitude estimators
37  *
38  * @author Roman Bast <bapstroman@gmail.com>
39  * @author Siddharth Bharat Purohit <siddharthbharatpurohit@gmail.com>
40  *
41  */
42 
43 #include <matrix/math.hpp>
44 
45 namespace estimator
46 {
47 
48 using matrix::AxisAnglef;
49 using matrix::Dcmf;
50 using matrix::Eulerf;
51 using matrix::Matrix3f;
52 using matrix::Quatf;
53 using matrix::Vector2f;
54 using matrix::Vector3f;
55 using matrix::wrap_pi;
56 
57 struct gps_message {
58  uint64_t time_usec;
59  int32_t lat; ///< Latitude in 1E-7 degrees
60  int32_t lon; ///< Longitude in 1E-7 degrees
61  int32_t alt; ///< Altitude in 1E-3 meters (millimeters) above MSL
62  float yaw; ///< yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
63  float yaw_offset; ///< Heading/Yaw offset for dual antenna GPS - refer to description for GPS_YAW_OFFSET
64  uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic
65  float eph; ///< GPS horizontal position accuracy in m
66  float epv; ///< GPS vertical position accuracy in m
67  float sacc; ///< GPS speed accuracy in m/s
68  float vel_m_s; ///< GPS ground speed (m/sec)
69  float vel_ned[3]; ///< GPS ground speed NED
70  bool vel_ned_valid; ///< GPS ground speed is valid
71  uint8_t nsats; ///< number of satellites used
72  float pdop; ///< position dilution of precision
73 };
74 
75 struct flow_message {
76  uint8_t quality; ///< Quality of Flow data
77  Vector2f flowdata; ///< Optical flow rates about the X and Y body axes (rad/sec)
78  Vector3f gyrodata; ///< Gyro rates about the XYZ body axes (rad/sec)
79  uint32_t dt; ///< integration time of flow samples (sec)
80 };
81 
83  Vector3f pos; ///< XYZ position in earth frame (m) - Z must be aligned with down axis
84  Vector3f vel; ///< XYZ velocity in earth frame (m/sec) - Z must be aligned with down axis
85  Quatf quat; ///< quaternion defining rotation from body to earth frame
86  float posErr; ///< 1-Sigma horizontal position accuracy (m)
87  float hgtErr; ///< 1-Sigma height accuracy (m)
88  float velErr; ///< 1-Sigma velocity accuracy (m/sec)
89  float angErr; ///< 1-Sigma angular error (rad)
90 };
91 
92 struct outputSample {
93  Quatf quat_nominal; ///< nominal quaternion describing vehicle attitude
94  Vector3f vel; ///< NED velocity estimate in earth frame (m/sec)
95  Vector3f pos; ///< NED position estimate in earth frame (m/sec)
96  uint64_t time_us; ///< timestamp of the measurement (uSec)
97 };
98 
99 struct outputVert {
100  float vel_d; ///< D velocity calculated using alternative algorithm (m/sec)
101  float vel_d_integ; ///< Integral of vel_d (m)
102  float dt; ///< delta time (sec)
103  uint64_t time_us; ///< timestamp of the measurement (uSec)
104 };
105 
106 struct imuSample {
107  Vector3f delta_ang; ///< delta angle in body frame (integrated gyro measurements) (rad)
108  Vector3f delta_vel; ///< delta velocity in body frame (integrated accelerometer measurements) (m/sec)
109  float delta_ang_dt; ///< delta angle integration period (sec)
110  float delta_vel_dt; ///< delta velocity integration period (sec)
111  uint64_t time_us; ///< timestamp of the measurement (uSec)
112 };
113 
114 struct gpsSample {
115  Vector2f pos; ///< NE earth frame gps horizontal position measurement (m)
116  float hgt; ///< gps height measurement (m)
117  Vector3f vel; ///< NED earth frame gps velocity measurement (m/sec)
118  float yaw; ///< yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
119  float hacc; ///< 1-std horizontal position error (m)
120  float vacc; ///< 1-std vertical position error (m)
121  float sacc; ///< 1-std speed error (m/sec)
122  uint64_t time_us; ///< timestamp of the measurement (uSec)
123 };
124 
125 struct magSample {
126  Vector3f mag; ///< NED magnetometer body frame measurements (Gauss)
127  uint64_t time_us; ///< timestamp of the measurement (uSec)
128 };
129 
130 struct baroSample {
131  float hgt{0.0f}; ///< pressure altitude above sea level (m)
132  uint64_t time_us{0}; ///< timestamp of the measurement (uSec)
133 };
134 
135 struct rangeSample {
136  float rng; ///< range (distance to ground) measurement (m)
137  uint64_t time_us; ///< timestamp of the measurement (uSec)
138  int8_t quality; ///< Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality.
139 };
140 
142  float true_airspeed; ///< true airspeed measurement (m/sec)
143  float eas2tas; ///< equivalent to true airspeed factor
144  uint64_t time_us; ///< timestamp of the measurement (uSec)
145 };
146 
147 struct flowSample {
148  uint8_t quality; ///< quality indicator between 0 and 255
149  Vector2f flowRadXY; ///< measured delta angle of the image about the X and Y body axes (rad), RH rotation is positive
150  Vector3f gyroXYZ; ///< measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (rad), RH rotation is positive
151  float dt; ///< amount of integration time (sec)
152  uint64_t time_us; ///< timestamp of the integration period leading edge (uSec)
153 };
154 
156  Vector3f pos; ///< XYZ position in earth frame (m) - Z must be aligned with down axis
157  Vector3f vel; ///< XYZ velocity in earth frame (m/sec) - Z must be aligned with down axis
158  Quatf quat; ///< quaternion defining rotation from body to earth frame
159  float posErr; ///< 1-Sigma horizontal position accuracy (m)
160  float hgtErr; ///< 1-Sigma height accuracy (m)
161  float velErr; ///< 1-Sigma velocity accuracy (m/sec)
162  float angErr; ///< 1-Sigma angular error (rad)
163  uint64_t time_us; ///< timestamp of the measurement (uSec)
164 };
165 
166 struct dragSample {
167  Vector2f accelXY; ///< measured specific force along the X and Y body axes (m/sec**2)
168  uint64_t time_us; ///< timestamp of the measurement (uSec)
169 };
170 
171 struct auxVelSample {
172  Vector2f velNE; ///< measured NE velocity relative to the local origin (m/sec)
173  Vector2f velVarNE; ///< estimated error variance of the NE velocity (m/sec)**2
174  uint64_t time_us; ///< timestamp of the measurement (uSec)
175 };
176 
177 // Integer definitions for vdist_sensor_type
178 #define VDIST_SENSOR_BARO 0 ///< Use baro height
179 #define VDIST_SENSOR_GPS 1 ///< Use GPS height
180 #define VDIST_SENSOR_RANGE 2 ///< Use range finder height
181 #define VDIST_SENSOR_EV 3 ///< Use external vision
182 
183 // Bit locations for mag_declination_source
184 #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
185 #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
186 #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
187 
188 // Bit locations for fusion_mode
189 #define MASK_USE_GPS (1<<0) ///< set to true to use GPS data
190 #define MASK_USE_OF (1<<1) ///< set to true to use optical flow data
191 #define MASK_INHIBIT_ACC_BIAS (1<<2) ///< set to true to inhibit estimation of accelerometer delta velocity bias
192 #define MASK_USE_EVPOS (1<<3) ///< set to true to use external vision position data
193 #define MASK_USE_EVYAW (1<<4) ///< set to true to use external vision quaternion data for yaw
194 #define MASK_USE_DRAG (1<<5) ///< set to true to use the multi-rotor drag model to estimate wind
195 #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
196 #define MASK_USE_GPSYAW (1<<7) ///< set to true to use GPS yaw data if available
197 #define MASK_USE_EVVEL (1<<8) ///< sset to true to use external vision velocity data
198 
199 // Integer definitions for mag_fusion_type
200 #define MAG_FUSE_TYPE_AUTO 0 ///< The selection of either heading or 3D magnetometer fusion will be automatic
201 #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
202 #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
203 #define MAG_FUSE_TYPE_UNUSED 3 ///< Not implemented
204 #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.
205 #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.
206 
207 // Maximum sensor intervals in usec
208 #define GPS_MAX_INTERVAL (uint64_t)5e5 ///< Maximum allowable time interval between GPS measurements (uSec)
209 #define BARO_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between pressure altitude measurements (uSec)
210 #define RNG_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between range finder measurements (uSec)
211 #define EV_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between external vision system measurements (uSec)
212 
213 // bad accelerometer detection and mitigation
214 #define BADACC_PROBATION (uint64_t)10e6 ///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec)
215 #define BADACC_BIAS_PNOISE 4.9f ///< The delta velocity process noise is set to this when accel data is declared bad (m/sec**2)
216 
217 // ground effect compensation
218 #define GNDEFFECT_TIMEOUT 10E6 ///< Maximum period of time that ground effect protection will be active after it was last turned on (uSec)
219 
220 struct parameters {
221  // measurement source control
222  int32_t fusion_mode{MASK_USE_GPS}; ///< bitmasked integer that selects which aiding sources will be used
223  int32_t vdist_sensor_type{VDIST_SENSOR_BARO}; ///< selects the primary source for height data
224  int32_t sensor_interval_min_ms{20}; ///< minimum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers. (mSec)
225 
226  // measurement time delays
227  float min_delay_ms{0.0f}; ///< Maximum time delay of any sensor used to increase buffer length to handle large timing jitter (mSec)
228  float mag_delay_ms{0.0f}; ///< magnetometer measurement delay relative to the IMU (mSec)
229  float baro_delay_ms{0.0f}; ///< barometer height measurement delay relative to the IMU (mSec)
230  float gps_delay_ms{110.0f}; ///< GPS measurement delay relative to the IMU (mSec)
231  float airspeed_delay_ms{100.0f}; ///< airspeed measurement delay relative to the IMU (mSec)
232  float flow_delay_ms{5.0f}; ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
233  float range_delay_ms{5.0f}; ///< range finder measurement delay relative to the IMU (mSec)
234  float ev_delay_ms{100.0f}; ///< off-board vision measurement delay relative to the IMU (mSec)
235  float auxvel_delay_ms{0.0f}; ///< auxiliary velocity measurement delay relative to the IMU (mSec)
236 
237  // input noise
238  float gyro_noise{1.5e-2f}; ///< IMU angular rate noise used for covariance prediction (rad/sec)
239  float accel_noise{3.5e-1f}; ///< IMU acceleration noise use for covariance prediction (m/sec**2)
240 
241  // process noise
242  float gyro_bias_p_noise{1.0e-3f}; ///< process noise for IMU rate gyro bias prediction (rad/sec**2)
243  float accel_bias_p_noise{6.0e-3f}; ///< process noise for IMU accelerometer bias prediction (m/sec**3)
244  float mage_p_noise{1.0e-3f}; ///< process noise for earth magnetic field prediction (Gauss/sec)
245  float magb_p_noise{1.0e-4f}; ///< process noise for body magnetic field prediction (Gauss/sec)
246  float wind_vel_p_noise{1.0e-1f}; ///< process noise for wind velocity prediction (m/sec**2)
247  float wind_vel_p_noise_scaler{0.5f}; ///< scaling of wind process noise with vertical velocity
248  float terrain_p_noise{5.0f}; ///< process noise for terrain offset (m/sec)
249  float terrain_gradient{0.5f}; ///< gradient of terrain used to estimate process noise due to changing position (m/m)
250 
251  // initialization errors
252  float switch_on_gyro_bias{0.1f}; ///< 1-sigma gyro bias uncertainty at switch on (rad/sec)
253  float switch_on_accel_bias{0.2f}; ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2)
254  float initial_tilt_err{0.1f}; ///< 1-sigma tilt error after initial alignment using gravity vector (rad)
255  float initial_wind_uncertainty{1.0f}; ///< 1-sigma initial uncertainty in wind velocity (m/sec)
256 
257  // position and velocity fusion
258  float gps_vel_noise{5.0e-1f}; ///< minimum allowed observation noise for gps velocity fusion (m/sec)
259  float gps_pos_noise{0.5f}; ///< minimum allowed observation noise for gps position fusion (m)
260  float pos_noaid_noise{10.0f}; ///< observation noise for non-aiding position fusion (m)
261  float baro_noise{2.0f}; ///< observation noise for barometric height fusion (m)
262  float baro_innov_gate{5.0f}; ///< barometric and GPS height innovation consistency gate size (STD)
263  float gps_pos_innov_gate{5.0f}; ///< GPS horizontal position innovation consistency gate size (STD)
264  float gps_vel_innov_gate{5.0f}; ///< GPS velocity innovation consistency gate size (STD)
265  float gnd_effect_deadzone{5.0f}; ///< Size of deadzone applied to negative baro innovations when ground effect compensation is active (m)
266  float gnd_effect_max_hgt{0.5f}; ///< Height above ground at which baro ground effect becomes insignificant (m)
267 
268  // magnetometer fusion
269  float mag_heading_noise{3.0e-1f}; ///< measurement noise used for simple heading fusion (rad)
270  float mag_noise{5.0e-2f}; ///< measurement noise used for 3-axis magnetoemeter fusion (Gauss)
271  float mag_declination_deg{0.0f}; ///< magnetic declination (degrees)
272  float heading_innov_gate{2.6f}; ///< heading fusion innovation consistency gate size (STD)
273  float mag_innov_gate{3.0f}; ///< magnetometer fusion innovation consistency gate size (STD)
274  int32_t mag_declination_source{7}; ///< bitmask used to control the handling of declination data
275  int32_t mag_fusion_type{0}; ///< integer used to specify the type of magnetometer fusion used
276  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)
277  float mag_yaw_rate_gate{0.25f}; ///< yaw rate threshold used by mode select logic (rad/sec)
278 
279  // airspeed fusion
280  float tas_innov_gate{5.0f}; ///< True Airspeed innovation consistency gate size (STD)
281  float eas_noise{1.4f}; ///< EAS measurement noise standard deviation used for airspeed fusion (m/s)
282 
283  // synthetic sideslip fusion
284  float beta_innov_gate{5.0f}; ///< synthetic sideslip innovation consistency gate size in standard deviation (STD)
285  float beta_noise{0.3f}; ///< synthetic sideslip noise (rad)
286  float beta_avg_ft_us{150000.0f}; ///< The average time between synthetic sideslip measurements (uSec)
287 
288  // range finder fusion
289  float range_noise{0.1f}; ///< observation noise for range finder measurements (m)
290  float range_innov_gate{5.0f}; ///< range finder fusion innovation consistency gate size (STD)
291  float rng_gnd_clearance{0.1f}; ///< minimum valid value for range when on ground (m)
292  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.
293  float range_noise_scaler{0.0f}; ///< scaling from range measurement to noise (m/m)
294  float vehicle_variance_scaler{0.0f}; ///< gain applied to vehicle height variance used in calculation of height above ground observation variance
295  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)
296  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)
297  int32_t range_aid{0}; ///< allow switching primary height source to range finder if certain conditions are met
298  float range_aid_innov_gate{1.0f}; ///< gate size used for innovation consistency checks for range aid fusion
299  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
300  float range_stuck_threshold{0.1f}; ///< minimum variation in range finder reading required to declare a range finder 'unstuck' when readings recommence after being out of range (m)
301  int32_t range_signal_hysteresis_ms{1000}; ///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (ms)
302 
303  // vision position fusion
304  float ev_vel_innov_gate{3.0f}; ///< vision velocity fusion innovation consistency gate size (STD)
305  float ev_pos_innov_gate{5.0f}; ///< vision position fusion innovation consistency gate size (STD)
306 
307  // optical flow fusion
308  float flow_noise{0.15f}; ///< observation noise for optical flow LOS rate measurements (rad/sec)
309  float flow_noise_qual_min{0.5f}; ///< observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec)
310  int32_t flow_qual_min{1}; ///< minimum acceptable quality integer from the flow sensor
311  float flow_innov_gate{3.0f}; ///< optical flow fusion innovation consistency gate size (STD)
312 
313  // these parameters control the strictness of GPS quality checks used to determine if the GPS is
314  // good enough to set a local origin and commence aiding
315  int32_t gps_check_mask{21}; ///< bitmask used to control which GPS quality checks are used
316  float req_hacc{5.0f}; ///< maximum acceptable horizontal position error (m)
317  float req_vacc{8.0f}; ///< maximum acceptable vertical position error (m)
318  float req_sacc{1.0f}; ///< maximum acceptable speed error (m/s)
319  int32_t req_nsats{6}; ///< minimum acceptable satellite count
320  float req_pdop{2.0f}; ///< maximum acceptable position dilution of precision
321  float req_hdrift{0.3f}; ///< maximum acceptable horizontal drift speed (m/s)
322  float req_vdrift{0.5f}; ///< maximum acceptable vertical drift speed (m/s)
323 
324  // XYZ offset of sensors in body axes (m)
325  Vector3f imu_pos_body; ///< xyz position of IMU in body frame (m)
326  Vector3f gps_pos_body; ///< xyz position of the GPS antenna in body frame (m)
327  Vector3f rng_pos_body; ///< xyz position of range sensor in body frame (m)
328  Vector3f flow_pos_body; ///< xyz position of range sensor focal point in body frame (m)
329  Vector3f ev_pos_body; ///< xyz position of VI-sensor focal point in body frame (m)
330 
331  // output complementary filter tuning
332  float vel_Tau{0.25f}; ///< velocity state correction time constant (1/sec)
333  float pos_Tau{0.25f}; ///< position state correction time constant (1/sec)
334 
335  // accel bias learning control
336  float acc_bias_lim{0.4f}; ///< maximum accel bias magnitude (m/sec**2)
337  float acc_bias_learn_acc_lim{25.0f}; ///< learning is disabled if the magnitude of the IMU acceleration vector is greater than this (m/sec**2)
338  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)
339  float acc_bias_learn_tc{0.5f}; ///< time constant used to control the decaying envelope filters applied to the accel and gyro magnitudes (sec)
340 
341  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)
342  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)
343 
344  int32_t valid_timeout_max{5000000}; ///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec)
345 
346  // multi-rotor drag specific force fusion
347  float drag_noise{2.5f}; ///< observation noise variance for drag specific force measurements (m/sec**2)**2
348  float bcoef_x{25.0f}; ///< ballistic coefficient along the X-axis (kg/m**2)
349  float bcoef_y{25.0f}; ///< ballistic coefficient along the Y-axis (kg/m**2)
350 
351  // control of accel error detection and mitigation (IMU clipping)
352  float vert_innov_test_lim{4.5f}; ///< Number of standard deviations allowed before the combined vertical velocity and position test is declared as failed
353  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)
354 
355  // auxiliary velocity fusion
356  float auxvel_noise{0.5f}; ///< minimum observation noise, uses reported noise if greater (m/s)
357  float auxvel_gate{5.0f}; ///< velocity fusion innovation consistency gate size (STD)
358 
359  // control of on-ground movement check
360  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.
361 
362  // compute synthetic magnetomter Z value if possible
363  int32_t synthesize_mag_z{0};
364  int32_t check_mag_strength{0};
365 };
366 
367 struct stateSample {
368  Quatf quat_nominal; ///< quaternion defining the rotation from body to earth frame
369  Vector3f vel; ///< NED velocity in earth frame in m/s
370  Vector3f pos; ///< NED position in earth frame in m
371  Vector3f gyro_bias; ///< delta angle bias estimate in rad
372  Vector3f accel_bias; ///< delta velocity bias estimate in m/s
373  Vector3f mag_I; ///< NED earth magnetic field in gauss
374  Vector3f mag_B; ///< magnetometer bias estimate in body frame in gauss
375  Vector2f wind_vel; ///< wind velocity in m/s
376 };
377 
379  struct {
380  bool bad_mag_x: 1; ///< 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error
381  bool bad_mag_y: 1; ///< 1 - true if the fusion of the magnetometer Y-axis has encountered a numerical error
382  bool bad_mag_z: 1; ///< 2 - true if the fusion of the magnetometer Z-axis has encountered a numerical error
383  bool bad_hdg: 1; ///< 3 - true if the fusion of the heading angle has encountered a numerical error
384  bool bad_mag_decl: 1; ///< 4 - true if the fusion of the magnetic declination has encountered a numerical error
385  bool bad_airspeed: 1; ///< 5 - true if fusion of the airspeed has encountered a numerical error
386  bool bad_sideslip: 1; ///< 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error
387  bool bad_optflow_X: 1; ///< 7 - true if fusion of the optical flow X axis has encountered a numerical error
388  bool bad_optflow_Y: 1; ///< 8 - true if fusion of the optical flow Y axis has encountered a numerical error
389  bool bad_vel_N: 1; ///< 9 - true if fusion of the North velocity has encountered a numerical error
390  bool bad_vel_E: 1; ///< 10 - true if fusion of the East velocity has encountered a numerical error
391  bool bad_vel_D: 1; ///< 11 - true if fusion of the Down velocity has encountered a numerical error
392  bool bad_pos_N: 1; ///< 12 - true if fusion of the North position has encountered a numerical error
393  bool bad_pos_E: 1; ///< 13 - true if fusion of the East position has encountered a numerical error
394  bool bad_pos_D: 1; ///< 14 - true if fusion of the Down position has encountered a numerical error
395  bool bad_acc_bias: 1; ///< 15 - true if bad delta velocity bias estimates have been detected
396  } flags;
397  uint16_t value;
398 
399 };
400 
401 // define structure used to communicate innovation test failures
403  struct {
404  bool reject_vel_NED: 1; ///< 0 - true if velocity observations have been rejected
405  bool reject_pos_NE: 1; ///< 1 - true if horizontal position observations have been rejected
406  bool reject_pos_D: 1; ///< 2 - true if true if vertical position observations have been rejected
407  bool reject_mag_x: 1; ///< 3 - true if the X magnetometer observation has been rejected
408  bool reject_mag_y: 1; ///< 4 - true if the Y magnetometer observation has been rejected
409  bool reject_mag_z: 1; ///< 5 - true if the Z magnetometer observation has been rejected
410  bool reject_yaw: 1; ///< 6 - true if the yaw observation has been rejected
411  bool reject_airspeed: 1; ///< 7 - true if the airspeed observation has been rejected
412  bool reject_sideslip: 1; ///< 8 - true if the synthetic sideslip observation has been rejected
413  bool reject_hagl: 1; ///< 9 - true if the height above ground observation has been rejected
414  bool reject_optflow_X: 1; ///< 10 - true if the X optical flow observation has been rejected
415  bool reject_optflow_Y: 1; ///< 11 - true if the Y optical flow observation has been rejected
416  } flags;
417  uint16_t value;
418 
419 };
420 
421 // publish the status of various GPS quality checks
423  struct {
424  uint16_t fix : 1; ///< 0 - true if the fix type is insufficient (no 3D solution)
425  uint16_t nsats : 1; ///< 1 - true if number of satellites used is insufficient
426  uint16_t pdop : 1; ///< 2 - true if position dilution of precision is insufficient
427  uint16_t hacc : 1; ///< 3 - true if reported horizontal accuracy is insufficient
428  uint16_t vacc : 1; ///< 4 - true if reported vertical accuracy is insufficient
429  uint16_t sacc : 1; ///< 5 - true if reported speed accuracy is insufficient
430  uint16_t hdrift : 1; ///< 6 - true if horizontal drift is excessive (can only be used when stationary on ground)
431  uint16_t vdrift : 1; ///< 7 - true if vertical drift is excessive (can only be used when stationary on ground)
432  uint16_t hspeed : 1; ///< 8 - true if horizontal speed is excessive (can only be used when stationary on ground)
433  uint16_t vspeed : 1; ///< 9 - true if vertical speed error is excessive
434  } flags;
435  uint16_t value;
436 };
437 
438 // bitmask containing filter control status
440  struct {
441  uint32_t tilt_align : 1; ///< 0 - true if the filter tilt alignment is complete
442  uint32_t yaw_align : 1; ///< 1 - true if the filter yaw alignment is complete
443  uint32_t gps : 1; ///< 2 - true if GPS measurements are being fused
444  uint32_t opt_flow : 1; ///< 3 - true if optical flow measurements are being fused
445  uint32_t mag_hdg : 1; ///< 4 - true if a simple magnetic yaw heading is being fused
446  uint32_t mag_3D : 1; ///< 5 - true if 3-axis magnetometer measurement are being fused
447  uint32_t mag_dec : 1; ///< 6 - true if synthetic magnetic declination measurements are being fused
448  uint32_t in_air : 1; ///< 7 - true when the vehicle is airborne
449  uint32_t wind : 1; ///< 8 - true when wind velocity is being estimated
450  uint32_t baro_hgt : 1; ///< 9 - true when baro height is being fused as a primary height reference
451  uint32_t rng_hgt : 1; ///< 10 - true when range finder height is being fused as a primary height reference
452  uint32_t gps_hgt : 1; ///< 11 - true when GPS height is being fused as a primary height reference
453  uint32_t ev_pos : 1; ///< 12 - true when local position data from external vision is being fused
454  uint32_t ev_yaw : 1; ///< 13 - true when yaw data from external vision measurements is being fused
455  uint32_t ev_hgt : 1; ///< 14 - true when height data from external vision measurements is being fused
456  uint32_t fuse_beta : 1; ///< 15 - true when synthetic sideslip measurements are being fused
457  uint32_t mag_field_disturbed : 1; ///< 16 - true when the mag field does not match the expected strength
458  uint32_t fixed_wing : 1; ///< 17 - true when the vehicle is operating as a fixed wing vehicle
459  uint32_t mag_fault : 1; ///< 18 - true when the magnetometer has been declared faulty and is no longer being used
460  uint32_t fuse_aspd : 1; ///< 19 - true when airspeed measurements are being fused
461  uint32_t gnd_effect : 1; ///< 20 - true when protection from ground effect induced static pressure rise is active
462  uint32_t rng_stuck : 1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough
463  uint32_t gps_yaw : 1; ///< 22 - true when yaw (not ground course) data from a GPS receiver is being fused
464  uint32_t mag_aligned_in_flight : 1; ///< 23 - true when the in-flight mag field alignment has been completed
465  uint32_t ev_vel : 1; ///< 24 - true when local earth frame velocity data from external vision measurements are being fused
466  uint32_t synthetic_mag_z : 1; ///< 25 - true when we are using a synthesized measurement for the magnetometer Z component
467  } flags;
468  uint32_t value;
469 };
470 
472  struct {
473  uint16_t attitude : 1; ///< 0 - True if the attitude estimate is good
474  uint16_t velocity_horiz : 1; ///< 1 - True if the horizontal velocity estimate is good
475  uint16_t velocity_vert : 1; ///< 2 - True if the vertical velocity estimate is good
476  uint16_t pos_horiz_rel : 1; ///< 3 - True if the horizontal position (relative) estimate is good
477  uint16_t pos_horiz_abs : 1; ///< 4 - True if the horizontal position (absolute) estimate is good
478  uint16_t pos_vert_abs : 1; ///< 5 - True if the vertical position (absolute) estimate is good
479  uint16_t pos_vert_agl : 1; ///< 6 - True if the vertical position (above ground) estimate is good
480  uint16_t const_pos_mode : 1; ///< 7 - True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow)
481  uint16_t pred_pos_horiz_rel : 1; ///< 8 - True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate
482  uint16_t pred_pos_horiz_abs : 1; ///< 9 - True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate
483  uint16_t gps_glitch : 1; ///< 10 - True if the EKF has detected a GPS glitch
484  uint16_t accel_error : 1; ///< 11 - True if the EKF has detected bad accelerometer data
485  } flags;
486  uint16_t value;
487 };
488 
489 }
float eph
GPS horizontal position accuracy in m.
Definition: common.h:65
uint16_t velocity_horiz
1 - True if the horizontal velocity estimate is good
Definition: common.h:474
Vector3f delta_ang
delta angle in body frame (integrated gyro measurements) (rad)
Definition: common.h:107
float yaw_offset
Heading/Yaw offset for dual antenna GPS - refer to description for GPS_YAW_OFFSET.
Definition: common.h:63
float hgt
gps height measurement (m)
Definition: common.h:116
uint32_t gps_yaw
22 - true when yaw (not ground course) data from a GPS receiver is being fused
Definition: common.h:463
bool reject_vel_NED
0 - true if velocity observations have been rejected
Definition: common.h:404
float epv
GPS vertical position accuracy in m.
Definition: common.h:66
bool reject_pos_NE
1 - true if horizontal position observations have been rejected
Definition: common.h:405
uint32_t yaw_align
1 - true if the filter yaw alignment is complete
Definition: common.h:442
uint8_t nsats
number of satellites used
Definition: common.h:71
bool bad_mag_x
0 - true if the fusion of the magnetometer X-axis has encountered a numerical error ...
Definition: common.h:380
float dt
delta time (sec)
Definition: common.h:102
uint64_t time_usec
Definition: common.h:58
float vel_m_s
GPS ground speed (m/sec)
Definition: common.h:68
bool bad_mag_y
1 - true if the fusion of the magnetometer Y-axis has encountered a numerical error ...
Definition: common.h:381
bool bad_optflow_X
7 - true if fusion of the optical flow X axis has encountered a numerical error
Definition: common.h:387
bool bad_pos_D
14 - true if fusion of the Down position has encountered a numerical error
Definition: common.h:394
Vector3f ev_pos_body
xyz position of VI-sensor focal point in body frame (m)
Definition: common.h:329
bool reject_pos_D
2 - true if true if vertical position observations have been rejected
Definition: common.h:406
uint16_t pos_horiz_abs
4 - True if the horizontal position (absolute) estimate is good
Definition: common.h:477
Dcm< float > Dcmf
Definition: Dcm.hpp:185
Vector3f accel_bias
delta velocity bias estimate in m/s
Definition: common.h:372
uint16_t nsats
1 - true if number of satellites used is insufficient
Definition: common.h:425
bool bad_mag_decl
4 - true if the fusion of the magnetic declination has encountered a numerical error ...
Definition: common.h:384
uint32_t fuse_aspd
19 - true when airspeed measurements are being fused
Definition: common.h:460
Vector2f pos
NE earth frame gps horizontal position measurement (m)
Definition: common.h:115
uint32_t fixed_wing
17 - true when the vehicle is operating as a fixed wing vehicle
Definition: common.h:458
bool reject_mag_x
3 - true if the X magnetometer observation has been rejected
Definition: common.h:407
Quatf quat
quaternion defining rotation from body to earth frame
Definition: common.h:158
uint32_t mag_hdg
4 - true if a simple magnetic yaw heading is being fused
Definition: common.h:445
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...
Definition: common.h:481
uint64_t time_us
timestamp of the measurement (uSec)
Definition: common.h:96
uint32_t mag_fault
18 - true when the magnetometer has been declared faulty and is no longer being used ...
Definition: common.h:459
float sacc
GPS speed accuracy in m/s.
Definition: common.h:67
bool reject_hagl
9 - true if the height above ground observation has been rejected
Definition: common.h:413
Vector2f accelXY
measured specific force along the X and Y body axes (m/sec**2)
Definition: common.h:167
Vector3f gyrodata
Gyro rates about the XYZ body axes (rad/sec)
Definition: common.h:78
uint16_t hdrift
6 - true if horizontal drift is excessive (can only be used when stationary on ground) ...
Definition: common.h:430
uint32_t rng_hgt
10 - true when range finder height is being fused as a primary height reference
Definition: common.h:451
bool bad_vel_N
9 - true if fusion of the North velocity has encountered a numerical error
Definition: common.h:389
uint64_t time_us
timestamp of the measurement (uSec)
Definition: common.h:122
uint16_t pos_vert_abs
5 - True if the vertical position (absolute) estimate is good
Definition: common.h:478
uint8_t fix_type
0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic ...
Definition: common.h:64
Vector3f mag_I
NED earth magnetic field in gauss.
Definition: common.h:373
uint32_t mag_field_disturbed
16 - true when the mag field does not match the expected strength
Definition: common.h:457
bool reject_mag_z
5 - true if the Z magnetometer observation has been rejected
Definition: common.h:409
bool bad_pos_N
12 - true if fusion of the North position has encountered a numerical error
Definition: common.h:392
uint32_t baro_hgt
9 - true when baro height is being fused as a primary height reference
Definition: common.h:450
float pdop
position dilution of precision
Definition: common.h:72
uint16_t sacc
5 - true if reported speed accuracy is insufficient
Definition: common.h:429
float vacc
1-std vertical position error (m)
Definition: common.h:120
uint16_t vacc
4 - true if reported vertical accuracy is insufficient
Definition: common.h:428
uint16_t velocity_vert
2 - True if the vertical velocity estimate is good
Definition: common.h:475
Vector3f gyroXYZ
measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (...
Definition: common.h:150
float vel_ned[3]
GPS ground speed NED.
Definition: common.h:69
uint32_t dt
integration time of flow samples (sec)
Definition: common.h:79
float hgtErr
1-Sigma height accuracy (m)
Definition: common.h:160
float dt
amount of integration time (sec)
Definition: common.h:151
uint16_t vspeed
9 - true if vertical speed error is excessive
Definition: common.h:433
Vector3f pos
XYZ position in earth frame (m) - Z must be aligned with down axis.
Definition: common.h:156
uint8_t quality
Quality of Flow data.
Definition: common.h:76
float vel_d
D velocity calculated using alternative algorithm (m/sec)
Definition: common.h:100
float rng
range (distance to ground) measurement (m)
Definition: common.h:136
Vector2f velVarNE
estimated error variance of the NE velocity (m/sec)**2
Definition: common.h:173
float yaw
yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
Definition: common.h:118
float sacc
1-std speed error (m/sec)
Definition: common.h:121
int8_t quality
Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality.
Definition: common.h:138
uint32_t synthetic_mag_z
25 - true when we are using a synthesized measurement for the magnetometer Z component ...
Definition: common.h:466
float velErr
1-Sigma velocity accuracy (m/sec)
Definition: common.h:88
bool bad_mag_z
2 - true if the fusion of the magnetometer Z-axis has encountered a numerical error ...
Definition: common.h:382
uint64_t time_us
timestamp of the measurement (uSec)
Definition: common.h:144
bool bad_acc_bias
15 - true if bad delta velocity bias estimates have been detected
Definition: common.h:395
uint32_t ev_vel
24 - true when local earth frame velocity data from external vision measurements are being fused ...
Definition: common.h:465
Vector3f rng_pos_body
xyz position of range sensor in body frame (m)
Definition: common.h:327
uint16_t pdop
2 - true if position dilution of precision is insufficient
Definition: common.h:426
uint64_t time_us
timestamp of the measurement (uSec)
Definition: common.h:163
uint16_t vdrift
7 - true if vertical drift is excessive (can only be used when stationary on ground) ...
Definition: common.h:431
float hgtErr
1-Sigma height accuracy (m)
Definition: common.h:87
uint64_t time_us
timestamp of the measurement (uSec)
Definition: common.h:174
float posErr
1-Sigma horizontal position accuracy (m)
Definition: common.h:159
uint32_t gnd_effect
20 - true when protection from ground effect induced static pressure rise is active ...
Definition: common.h:461
bool vel_ned_valid
GPS ground speed is valid.
Definition: common.h:70
Vector3f vel
NED earth frame gps velocity measurement (m/sec)
Definition: common.h:117
float eas2tas
equivalent to true airspeed factor
Definition: common.h:143
bool bad_airspeed
5 - true if fusion of the airspeed has encountered a numerical error
Definition: common.h:385
Vector3f flow_pos_body
xyz position of range sensor focal point in body frame (m)
Definition: common.h:328
uint64_t time_us
timestamp of the measurement (uSec)
Definition: common.h:111
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 ...
Definition: common.h:480
Vector3f pos
NED position estimate in earth frame (m/sec)
Definition: common.h:95
Vector3f gps_pos_body
xyz position of the GPS antenna in body frame (m)
Definition: common.h:326
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
bool bad_hdg
3 - true if the fusion of the heading angle has encountered a numerical error
Definition: common.h:383
uint16_t attitude
0 - True if the attitude estimate is good
Definition: common.h:473
Vector2< float > Vector2f
Definition: Vector2.hpp:69
uint64_t time_us
timestamp of the measurement (uSec)
Definition: common.h:103
uint16_t hspeed
8 - true if horizontal speed is excessive (can only be used when stationary on ground) ...
Definition: common.h:432
uint8_t quality
quality indicator between 0 and 255
Definition: common.h:148
Vector3f pos
XYZ position in earth frame (m) - Z must be aligned with down axis.
Definition: common.h:83
Euler< float > Eulerf
Definition: Euler.hpp:156
Vector3f vel
NED velocity in earth frame in m/s.
Definition: common.h:369
int32_t lat
Latitude in 1E-7 degrees.
Definition: common.h:59
uint16_t pos_horiz_rel
3 - True if the horizontal position (relative) estimate is good
Definition: common.h:476
uint32_t ev_hgt
14 - true when height data from external vision measurements is being fused
Definition: common.h:455
Vector2f flowRadXY
measured delta angle of the image about the X and Y body axes (rad), RH rotation is positive ...
Definition: common.h:149
uint32_t ev_yaw
13 - true when yaw data from external vision measurements is being fused
Definition: common.h:454
uint16_t pos_vert_agl
6 - True if the vertical position (above ground) estimate is good
Definition: common.h:479
bool reject_yaw
6 - true if the yaw observation has been rejected
Definition: common.h:410
bool reject_optflow_Y
11 - true if the Y optical flow observation has been rejected
Definition: common.h:415
Vector2f velNE
measured NE velocity relative to the local origin (m/sec)
Definition: common.h:172
uint32_t mag_aligned_in_flight
23 - true when the in-flight mag field alignment has been completed
Definition: common.h:464
Vector2f flowdata
Optical flow rates about the X and Y body axes (rad/sec)
Definition: common.h:77
#define MASK_USE_GPS
set to true to use GPS data
Definition: common.h:189
Type wrap_pi(Type x)
Wrap value in range [-π, π)
Quatf quat_nominal
quaternion defining the rotation from body to earth frame
Definition: common.h:368
Vector3f pos
NED position in earth frame in m.
Definition: common.h:370
Vector3f vel
XYZ velocity in earth frame (m/sec) - Z must be aligned with down axis.
Definition: common.h:84
uint32_t mag_dec
6 - true if synthetic magnetic declination measurements are being fused
Definition: common.h:447
Vector3< float > Vector3f
Definition: Vector3.hpp:136
float yaw
yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
Definition: common.h:62
uint32_t opt_flow
3 - true if optical flow measurements are being fused
Definition: common.h:444
float true_airspeed
true airspeed measurement (m/sec)
Definition: common.h:142
#define VDIST_SENSOR_BARO
Use baro height.
Definition: common.h:178
Vector3f mag_B
magnetometer bias estimate in body frame in gauss
Definition: common.h:374
uint16_t fix
0 - true if the fix type is insufficient (no 3D solution)
Definition: common.h:424
bool bad_vel_E
10 - true if fusion of the East velocity has encountered a numerical error
Definition: common.h:390
bool reject_optflow_X
10 - true if the X optical flow observation has been rejected
Definition: common.h:414
bool bad_sideslip
6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error ...
Definition: common.h:386
uint32_t mag_3D
5 - true if 3-axis magnetometer measurement are being fused
Definition: common.h:446
Quaternion< float > Quatf
Definition: Quaternion.hpp:544
uint64_t time_us
timestamp of the measurement (uSec)
Definition: common.h:127
float angErr
1-Sigma angular error (rad)
Definition: common.h:89
Vector3f vel
NED velocity estimate in earth frame (m/sec)
Definition: common.h:94
bool reject_sideslip
8 - true if the synthetic sideslip observation has been rejected
Definition: common.h:412
uint32_t in_air
7 - true when the vehicle is airborne
Definition: common.h:448
uint16_t hacc
3 - true if reported horizontal accuracy is insufficient
Definition: common.h:427
float vel_d_integ
Integral of vel_d (m)
Definition: common.h:101
bool reject_mag_y
4 - true if the Y magnetometer observation has been rejected
Definition: common.h:408
int32_t alt
Altitude in 1E-3 meters (millimeters) above MSL.
Definition: common.h:61
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...
Definition: common.h:482
uint32_t wind
8 - true when wind velocity is being estimated
Definition: common.h:449
Quatf quat
quaternion defining rotation from body to earth frame
Definition: common.h:85
uint32_t ev_pos
12 - true when local position data from external vision is being fused
Definition: common.h:453
Vector3f imu_pos_body
xyz position of IMU in body frame (m)
Definition: common.h:325
uint32_t fuse_beta
15 - true when synthetic sideslip measurements are being fused
Definition: common.h:456
float hacc
1-std horizontal position error (m)
Definition: common.h:119
AxisAngle< float > AxisAnglef
Definition: AxisAngle.hpp:160
Vector3f vel
XYZ velocity in earth frame (m/sec) - Z must be aligned with down axis.
Definition: common.h:157
uint32_t gps_hgt
11 - true when GPS height is being fused as a primary height reference
Definition: common.h:452
float angErr
1-Sigma angular error (rad)
Definition: common.h:162
uint32_t rng_stuck
21 - true when rng data wasn&#39;t ready for more than 10s and new rng values haven&#39;t changed enough ...
Definition: common.h:462
bool bad_optflow_Y
8 - true if fusion of the optical flow Y axis has encountered a numerical error
Definition: common.h:388
uint32_t gps
2 - true if GPS measurements are being fused
Definition: common.h:443
uint32_t tilt_align
0 - true if the filter tilt alignment is complete
Definition: common.h:441
Vector2f wind_vel
wind velocity in m/s
Definition: common.h:375
float velErr
1-Sigma velocity accuracy (m/sec)
Definition: common.h:161
Vector3f mag
NED magnetometer body frame measurements (Gauss)
Definition: common.h:126
bool reject_airspeed
7 - true if the airspeed observation has been rejected
Definition: common.h:411
Vector3f delta_vel
delta velocity in body frame (integrated accelerometer measurements) (m/sec)
Definition: common.h:108
Vector3f gyro_bias
delta angle bias estimate in rad
Definition: common.h:371
Quatf quat_nominal
nominal quaternion describing vehicle attitude
Definition: common.h:93
float delta_ang_dt
delta angle integration period (sec)
Definition: common.h:109
uint64_t time_us
timestamp of the integration period leading edge (uSec)
Definition: common.h:152
uint64_t time_us
timestamp of the measurement (uSec)
Definition: common.h:168
uint64_t time_us
timestamp of the measurement (uSec)
Definition: common.h:137
uint16_t accel_error
11 - True if the EKF has detected bad accelerometer data
Definition: common.h:484
float delta_vel_dt
delta velocity integration period (sec)
Definition: common.h:110
bool bad_vel_D
11 - true if fusion of the Down velocity has encountered a numerical error
Definition: common.h:391
bool bad_pos_E
13 - true if fusion of the East position has encountered a numerical error
Definition: common.h:393
int32_t lon
Longitude in 1E-7 degrees.
Definition: common.h:60
float posErr
1-Sigma horizontal position accuracy (m)
Definition: common.h:86
uint16_t gps_glitch
10 - True if the EKF has detected a GPS glitch
Definition: common.h:483