@ -81,14 +81,21 @@
@@ -81,14 +81,21 @@
# define GSF_YAW_VALID_HISTORY_THRESHOLD 5
// minimum variances allowed for velocity and position states
# define VEL_STATE_MIN_VARIANCE 1E-4f
# define POS_STATE_MIN_VARIANCE 1E-4f
# define VEL_STATE_MIN_VARIANCE 1E-4
# define POS_STATE_MIN_VARIANCE 1E-4
// maximum number of times the vertical velocity variance can hit the lower limit before the
// associated states, variances and covariances are reset
# define EKF_TARGET_RATE_HZ uint32_t(1.0f / EKF_TARGET_DT)
# define EKF_TARGET_RATE_HZ uint32_t(1.0 / EKF_TARGET_DT)
# define VERT_VEL_VAR_CLIP_COUNT_LIM (5 * EKF_TARGET_RATE_HZ)
// limit on horizontal position states
# if HAL_WITH_EKF_DOUBLE
# define EK3_POSXY_STATE_LIMIT 50.0e6
# else
# define EK3_POSXY_STATE_LIMIT 1.0e6
# endif
class NavEKF3_core : public NavEKF_core_common
{
public :
@ -407,7 +414,6 @@ private:
@@ -407,7 +414,6 @@ private:
uint8_t imu_buffer_length ;
uint8_t obs_buffer_length ;
typedef float ftype ;
# if MATH_CHECK_INDEXES
typedef VectorN < ftype , 2 > Vector2 ;
typedef VectorN < ftype , 3 > Vector3 ;
@ -461,14 +467,14 @@ private:
@@ -461,14 +467,14 @@ private:
// broken down as individual elements. Both are equivalent (same
// memory)
struct state_elements {
Quaternion quat ; // quaternion defining rotation from local NED earth frame to body frame 0..3
Vector3f velocity ; // velocity of IMU in local NED earth frame (m/sec) 4..6
Vector3f position ; // position of IMU in local NED earth frame (m) 7..9
Vector3f gyro_bias ; // body frame delta angle IMU bias vector (rad) 10..12
Vector3f accel_bias ; // body frame delta velocity IMU bias vector (m/sec) 13..15
Vector3f earth_magfield ; // earth frame magnetic field vector (Gauss) 16..18
Vector3f body_magfield ; // body frame magnetic field vector (Gauss) 19..21
Vector2f wind_vel ; // horizontal North East wind velocity vector in local NED earth frame (m/sec) 22..23
QuaternionF quat ; // quaternion defining rotation from local NED earth frame to body frame 0..3
Vector3F velocity ; // velocity of IMU in local NED earth frame (m/sec) 4..6
Vector3F position ; // position of IMU in local NED earth frame (m) 7..9
Vector3F gyro_bias ; // body frame delta angle IMU bias vector (rad) 10..12
Vector3F accel_bias ; // body frame delta velocity IMU bias vector (m/sec) 13..15
Vector3F earth_magfield ; // earth frame magnetic field vector (Gauss) 16..18
Vector3F body_magfield ; // body frame magnetic field vector (Gauss) 19..21
Vector2F wind_vel ; // horizontal North East wind velocity vector in local NED earth frame (m/sec) 22..23
} ;
union {
@ -477,73 +483,73 @@ private:
@@ -477,73 +483,73 @@ private:
} ;
struct output_elements {
Quaternion quat ; // quaternion defining rotation from local NED earth frame to body frame
Vector3f velocity ; // velocity of body frame origin in local NED earth frame (m/sec)
Vector3f position ; // position of body frame origin in local NED earth frame (m)
QuaternionF quat ; // quaternion defining rotation from local NED earth frame to body frame
Vector3F velocity ; // velocity of body frame origin in local NED earth frame (m/sec)
Vector3F position ; // position of body frame origin in local NED earth frame (m)
} ;
struct imu_elements {
Vector3f delAng ; // IMU delta angle measurements in body frame (rad)
Vector3f delVel ; // IMU delta velocity measurements in body frame (m/sec)
float delAngDT ; // time interval over which delAng has been measured (sec)
float delVelDT ; // time interval over which delVelDT has been measured (sec)
Vector3F delAng ; // IMU delta angle measurements in body frame (rad)
Vector3F delVel ; // IMU delta velocity measurements in body frame (m/sec)
ftype delAngDT ; // time interval over which delAng has been measured (sec)
ftype delVelDT ; // time interval over which delVelDT has been measured (sec)
uint32_t time_ms ; // measurement timestamp (msec)
uint8_t gyro_index ;
uint8_t accel_index ;
} ;
struct gps_elements : EKF_obs_element_t {
Vector2f pos ; // horizontal North East position of the GPS antenna in local NED earth frame (m)
float hgt ; // height of the GPS antenna in local NED earth frame (m)
Vector3f vel ; // velocity of the GPS antenna in local NED earth frame (m/sec)
Vector2F pos ; // horizontal North East position of the GPS antenna in local NED earth frame (m)
ftype hgt ; // height of the GPS antenna in local NED earth frame (m)
Vector3F vel ; // velocity of the GPS antenna in local NED earth frame (m/sec)
uint8_t sensor_idx ; // unique integer identifying the GPS sensor
bool corrected ; // true when the position and velocity have been corrected for sensor position
bool have_vz ; // true when vertical velocity is valid
} ;
struct mag_elements : EKF_obs_element_t {
Vector3f mag ; // body frame magnetic field measurements (Gauss)
Vector3F mag ; // body frame magnetic field measurements (Gauss)
} ;
struct baro_elements : EKF_obs_element_t {
float hgt ; // height of the pressure sensor in local NED earth frame (m)
ftype hgt ; // height of the pressure sensor in local NED earth frame (m)
} ;
struct range_elements : EKF_obs_element_t {
float rng ; // distance measured by the range sensor (m)
ftype rng ; // distance measured by the range sensor (m)
uint8_t sensor_idx ; // integer either 0 or 1 uniquely identifying up to two range sensors
} ;
struct rng_bcn_elements : EKF_obs_element_t {
float rng ; // range measurement to each beacon (m)
Vector3f beacon_posNED ; // NED position of the beacon (m)
float rngErr ; // range measurement error 1-std (m)
ftype rng ; // range measurement to each beacon (m)
Vector3F beacon_posNED ; // NED position of the beacon (m)
ftype rngErr ; // range measurement error 1-std (m)
uint8_t beacon_ID ; // beacon identification number
} ;
struct tas_elements : EKF_obs_element_t {
float tas ; // true airspeed measurement (m/sec)
ftype tas ; // true airspeed measurement (m/sec)
} ;
struct of_elements : EKF_obs_element_t {
Vector2f flowRadXY ; // raw (non motion compensated) optical flow angular rates about the XY body axes (rad/sec)
Vector2f flowRadXYcomp ; // motion compensated XY optical flow angular rates about the XY body axes (rad/sec)
Vector3f bodyRadXYZ ; // body frame XYZ axis angular rates averaged across the optical flow measurement interval (rad/sec)
Vector3f body_offset ; // XYZ position of the optical flow sensor in body frame (m)
Vector2F flowRadXY ; // raw (non motion compensated) optical flow angular rates about the XY body axes (rad/sec)
Vector2F flowRadXYcomp ; // motion compensated XY optical flow angular rates about the XY body axes (rad/sec)
Vector3F bodyRadXYZ ; // body frame XYZ axis angular rates averaged across the optical flow measurement interval (rad/sec)
Vector3F body_offset ; // XYZ position of the optical flow sensor in body frame (m)
} ;
struct vel_odm_elements : EKF_obs_element_t {
Vector3f vel ; // XYZ velocity measured in body frame (m/s)
float velErr ; // velocity measurement error 1-std (m/s)
Vector3f body_offset ; // XYZ position of the velocity sensor in body frame (m)
Vector3f angRate ; // angular rate estimated from odometry (rad/sec)
Vector3F vel ; // XYZ velocity measured in body frame (m/s)
ftype velErr ; // velocity measurement error 1-std (m/s)
Vector3F body_offset ; // XYZ position of the velocity sensor in body frame (m)
Vector3F angRate ; // angular rate estimated from odometry (rad/sec)
} ;
struct wheel_odm_elements : EKF_obs_element_t {
float delAng ; // wheel rotation angle measured in body frame - positive is forward movement of vehicle (rad/s)
float radius ; // wheel radius (m)
Vector3f hub_offset ; // XYZ position of the wheel hub in body frame (m)
float delTime ; // time interval that the measurement was accumulated over (sec)
ftype delAng ; // wheel rotation angle measured in body frame - positive is forward movement of vehicle (rad/s)
ftype radius ; // wheel radius (m)
Vector3F hub_offset ; // XYZ position of the wheel hub in body frame (m)
ftype delTime ; // time interval that the measurement was accumulated over (sec)
} ;
// Specifies the rotation order used for the Tait-Bryan or Euler angles where alternative rotation orders are available
@ -553,21 +559,21 @@ private:
@@ -553,21 +559,21 @@ private:
} ;
struct yaw_elements : EKF_obs_element_t {
float yawAng ; // yaw angle measurement (rad)
float yawAngErr ; // yaw angle 1SD measurement accuracy (rad)
ftype yawAng ; // yaw angle measurement (rad)
ftype yawAngErr ; // yaw angle 1SD measurement accuracy (rad)
rotationOrder order ; // type specifiying Euler rotation order used, 0 = 321 (ZYX), 1 = 312 (ZXY)
} ;
struct ext_nav_elements : EKF_obs_element_t {
Vector3f pos ; // XYZ position measured in a RH navigation frame (m)
float posErr ; // spherical position measurement error 1-std (m)
Vector3F pos ; // XYZ position measured in a RH navigation frame (m)
ftype posErr ; // spherical position measurement error 1-std (m)
bool posReset ; // true when the position measurement has been reset
bool corrected ; // true when the position has been corrected for sensor position
} ;
struct ext_nav_vel_elements : EKF_obs_element_t {
Vector3f vel ; // velocity in NED (m/s)
float err ; // velocity measurement error (m/s)
Vector3F vel ; // velocity in NED (m/s)
ftype err ; // velocity measurement error (m/s)
bool corrected ; // true when the velocity has been corrected for sensor position
} ;
@ -578,8 +584,8 @@ private:
@@ -578,8 +584,8 @@ private:
// bias estimates for the IMUs that are enabled but not being used
// by this core.
struct {
Vector3f gyro_bias ;
Vector3f accel_bias ;
Vector3F gyro_bias ;
Vector3F accel_bias ;
} inactiveBias [ INS_MAX_INSTANCES ] ;
// Specify source of data to be used for a partial state reset
@ -614,7 +620,7 @@ private:
@@ -614,7 +620,7 @@ private:
// calculate the predicted state covariance matrix
// Argument rotVarVecPtr is pointer to a vector defining the earth frame uncertainty variance of the quaternion states
// used to perform a reset of the quaternion state covariances only. Set to null for normal operation.
void CovariancePrediction ( Vector3f * rotVarVecPtr ) ;
void CovariancePrediction ( Vector3F * rotVarVecPtr ) ;
// force symmetry on the state covariance matrix
void ForceSymmetry ( ) ;
@ -641,7 +647,7 @@ private:
@@ -641,7 +647,7 @@ private:
void FuseRngBcnStatic ( ) ;
// calculate the offset from EKF vertical position datum to the range beacon system datum
void CalcRangeBeaconPosDownOffset ( float obsVar , Vector3f & vehiclePosNED , bool aligning ) ;
void CalcRangeBeaconPosDownOffset ( ftype obsVar , Vector3F & vehiclePosNED , bool aligning ) ;
// fuse magnetometer measurements
void FuseMagnetometer ( ) ;
@ -665,21 +671,21 @@ private:
@@ -665,21 +671,21 @@ private:
void StoreQuatReset ( void ) ;
// Rotate the stored output quaternion history through a quaternion rotation
void StoreQuatRotate ( const Quaternion & deltaQuat ) ;
void StoreQuatRotate ( const QuaternionF & deltaQuat ) ;
// calculate the NED earth spin vector in rad/sec
void calcEarthRateNED ( Vector3f & omega , int32_t latitude ) const ;
void calcEarthRateNED ( Vector3F & omega , int32_t latitude ) const ;
// initialise the covariance matrix
void CovarianceInit ( ) ;
// helper functions for readIMUData
bool readDeltaVelocity ( uint8_t ins_index , Vector3f & dVel , float & dVel_dt ) ;
bool readDeltaAngle ( uint8_t ins_index , Vector3f & dAng , float & dAng_dt ) ;
bool readDeltaVelocity ( uint8_t ins_index , Vector3F & dVel , ftype & dVel_dt ) ;
bool readDeltaAngle ( uint8_t ins_index , Vector3F & dAng , ftype & dAng_dt ) ;
// helper functions for correcting IMU data
void correctDeltaAngle ( Vector3f & delAng , float delAngDT , uint8_t gyro_index ) ;
void correctDeltaVelocity ( Vector3f & delVel , float delVelDT , uint8_t accel_index ) ;
void correctDeltaAngle ( Vector3F & delAng , ftype delAngDT , uint8_t gyro_index ) ;
void correctDeltaVelocity ( Vector3F & delVel , ftype delVelDT , uint8_t accel_index ) ;
// update IMU delta angle and delta velocity measurements
void readIMUData ( ) ;
@ -744,10 +750,10 @@ private:
@@ -744,10 +750,10 @@ private:
void ResetPosition ( resetDataSource posResetSource ) ;
// reset the stateStruct's NE position to the specified position
void ResetPositionNE ( float posN , float posE ) ;
void ResetPositionNE ( ftype posN , ftype posE ) ;
// reset the stateStruct's D position
void ResetPositionD ( float posD ) ;
void ResetPositionD ( ftype posD ) ;
// reset velocity states using the last GPS measurement
void ResetVelocity ( resetDataSource velResetSource ) ;
@ -768,7 +774,7 @@ private:
@@ -768,7 +774,7 @@ private:
void checkDivergence ( void ) ;
// Calculate weighting that is applied to IMU1 accel data to blend data from IMU's 1 and 2
void calcIMU_Weighting ( float K1 , float K2 ) ;
void calcIMU_Weighting ( ftype K1 , ftype K2 ) ;
// return true if the filter is ready to start using optical flow measurements for position and velocity estimation
bool readyToUseOptFlow ( void ) const ;
@ -847,10 +853,10 @@ private:
@@ -847,10 +853,10 @@ private:
// Fuse declination angle to keep earth field declination from changing when we don't have earth relative observations.
// Input is 1-sigma uncertainty in published declination
void FuseDeclination ( float declErr ) ;
void FuseDeclination ( ftype declErr ) ;
// return magnetic declination in radians
float MagDeclination ( void ) const ;
ftype MagDeclination ( void ) const ;
// Propagate PVA solution forward from the fusion time horizon to the current time horizon
// using a simple observer
@ -905,7 +911,7 @@ private:
@@ -905,7 +911,7 @@ private:
// Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration
// Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration
// variances argument is updated with variances for each axis
void CalculateVelInnovationsAndVariances ( const Vector3f & velocity , float noise , float accel_scale , Vector3f & innovations , Vector3f & variances ) const ;
void CalculateVelInnovationsAndVariances ( const Vector3F & velocity , ftype noise , ftype accel_scale , Vector3F & innovations , Vector3F & variances ) const ;
// Runs the IMU prediction step for an independent GSF yaw estimator algorithm
// that uses IMU, GPS horizontal velocity and optionally true airspeed data.
@ -921,14 +927,14 @@ private:
@@ -921,14 +927,14 @@ private:
// yaw : new yaw angle (rad)
// yaw_variance : variance of new yaw angle (rad^2)
// order : enum defining Tait-Bryan rotation order used in calculation of the yaw angle
void resetQuatStateYawOnly ( float yaw , float yawVariance , rotationOrder order ) ;
void resetQuatStateYawOnly ( ftype yaw , ftype yawVariance , rotationOrder order ) ;
// attempt to reset the yaw to the EKF-GSF value
// returns false if unsuccessful
bool EKFGSF_resetMainFilterYaw ( ) ;
// returns true on success and populates yaw (in radians) and yawVariance (rad^2)
bool EKFGSF_getYaw ( float & yaw , float & yawVariance ) const ;
bool EKFGSF_getYaw ( ftype & yaw , ftype & yawVariance ) const ;
// Fusion of body frame X and Y axis drag specific forces for multi-rotor wind estimation
void FuseDragForces ( ) ;
@ -948,7 +954,7 @@ private:
@@ -948,7 +954,7 @@ private:
bool badIMUdata ; // boolean true if the bad IMU data is detected
uint32_t vertVelVarClipCounter ; // counter used to control reset of vertical velocity variance following collapse against the lower limit
float gpsNoiseScaler ; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts
ftype gpsNoiseScaler ; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts
Matrix24 P ; // covariance matrix
EKF_IMU_buffer_t < imu_elements > storedIMU ; // IMU data buffer
EKF_obs_buffer_t < gps_elements > storedGPS ; // GPS data buffer
@ -957,10 +963,10 @@ private:
@@ -957,10 +963,10 @@ private:
EKF_obs_buffer_t < tas_elements > storedTAS ; // TAS data buffer
EKF_obs_buffer_t < range_elements > storedRange ; // Range finder data buffer
EKF_IMU_buffer_t < output_elements > storedOutput ; // output state buffer
Matrix3f prevTnb ; // previous nav to body transformation used for INS earth rotation compensation
Matrix3F prevTnb ; // previous nav to body transformation used for INS earth rotation compensation
ftype accNavMag ; // magnitude of navigation accel - used to adjust GPS obs variance (m/s^2)
ftype accNavMagHoriz ; // magnitude of navigation accel in horizontal plane (m/s^2)
Vector3f earthRateNED ; // earths angular rate vector in NED (rad/s)
Vector3F earthRateNED ; // earths angular rate vector in NED (rad/s)
ftype dtIMUavg ; // expected time between IMU measurements (sec)
ftype dtEkfAvg ; // expected time between EKF updates (sec)
ftype dt ; // time lapsed since the last covariance prediction (sec)
@ -976,12 +982,12 @@ private:
@@ -976,12 +982,12 @@ private:
bool fuseVelData ; // this boolean causes the velNED measurements to be fused
bool fusePosData ; // this boolean causes the posNE measurements to be fused
bool fuseHgtData ; // this boolean causes the hgtMea measurements to be fused
Vector3f innovMag ; // innovation output from fusion of X,Y,Z compass measurements
Vector3f varInnovMag ; // innovation variance output from fusion of X,Y,Z compass measurements
Vector3F innovMag ; // innovation output from fusion of X,Y,Z compass measurements
Vector3F varInnovMag ; // innovation variance output from fusion of X,Y,Z compass measurements
ftype innovVtas ; // innovation output from fusion of airspeed measurements
ftype varInnovVtas ; // innovation variance output from fusion of airspeed measurements
float defaultAirSpeed ; // default equivalent airspeed in m/s to be used if the measurement is unavailable. Do not use if not positive.
float defaultAirSpeedVariance ; // default equivalent airspeed variance in (m/s)**2 to be used when defaultAirSpeed is specified.
ftype defaultAirSpeed ; // default equivalent airspeed in m/s to be used if the measurement is unavailable. Do not use if not positive.
ftype defaultAirSpeedVariance ; // default equivalent airspeed variance in (m/s)**2 to be used when defaultAirSpeed is specified.
bool magFusePerformed ; // boolean set to true when magnetometer fusion has been perfomred in that time step
MagCal effectiveMagCal ; // the actual mag calibration being used as the default
uint32_t prevTasStep_ms ; // time stamp of last TAS fusion step
@ -989,8 +995,8 @@ private:
@@ -989,8 +995,8 @@ private:
ftype innovBeta ; // synthetic sideslip innovation (rad)
uint32_t lastMagUpdate_us ; // last time compass was updated in usec
uint32_t lastMagRead_ms ; // last time compass data was successfully read
Vector3f velDotNED ; // rate of change of velocity in NED frame
Vector3f velDotNEDfilt ; // low pass filtered velDotNED
Vector3F velDotNED ; // rate of change of velocity in NED frame
Vector3F velDotNEDfilt ; // low pass filtered velDotNED
uint32_t imuSampleTime_ms ; // time that the last IMU value was taken
bool tasDataToFuse ; // true when new airspeed data is waiting to be fused
uint32_t lastBaroReceived_ms ; // time last time we received baro height data
@ -1005,13 +1011,13 @@ private:
@@ -1005,13 +1011,13 @@ private:
bool allMagSensorsFailed ; // true if all magnetometer sensors have timed out on this flight and we are no longer using magnetometer data
uint32_t lastSynthYawTime_ms ; // time stamp when yaw observation was last fused (msec)
uint32_t ekfStartTime_ms ; // time the EKF was started (msec)
Vector2f lastKnownPositionNE ; // last known position
Vector2F lastKnownPositionNE ; // last known position
uint32_t lastLaunchAccelTime_ms ;
float velTestRatio ; // sum of squares of GPS velocity innovation divided by fail threshold
float posTestRatio ; // sum of squares of GPS position innovation divided by fail threshold
float hgtTestRatio ; // sum of squares of baro height innovation divided by fail threshold
Vector3f magTestRatio ; // sum of squares of magnetometer innovations divided by fail threshold
float tasTestRatio ; // sum of squares of true airspeed innovation divided by fail threshold
ftype velTestRatio ; // sum of squares of GPS velocity innovation divided by fail threshold
ftype posTestRatio ; // sum of squares of GPS position innovation divided by fail threshold
ftype hgtTestRatio ; // sum of squares of baro height innovation divided by fail threshold
Vector3F magTestRatio ; // sum of squares of magnetometer innovations divided by fail threshold
ftype tasTestRatio ; // sum of squares of true airspeed innovation divided by fail threshold
bool inhibitWindStates ; // true when wind states and covariances are to remain constant
bool inhibitMagStates ; // true when magnetic field states are inactive
bool lastInhibitMagStates ; // previous inhibitMagStates
@ -1021,15 +1027,15 @@ private:
@@ -1021,15 +1027,15 @@ private:
bool gpsNotAvailable ; // bool true when valid GPS data is not available
struct Location EKF_origin ; // LLH origin of the NED axis system
bool validOrigin ; // true when the EKF origin is valid
float gpsSpdAccuracy ; // estimated speed accuracy in m/s returned by the GPS receiver
float gpsPosAccuracy ; // estimated position accuracy in m returned by the GPS receiver
float gpsHgtAccuracy ; // estimated height accuracy in m returned by the GPS receiver
ftype gpsSpdAccuracy ; // estimated speed accuracy in m/s returned by the GPS receiver
ftype gpsPosAccuracy ; // estimated position accuracy in m returned by the GPS receiver
ftype gpsHgtAccuracy ; // estimated height accuracy in m returned by the GPS receiver
uint32_t lastGpsVelFail_ms ; // time of last GPS vertical velocity consistency check fail
uint32_t lastGpsVelPass_ms ; // time of last GPS vertical velocity consistency check pass
uint32_t lastGpsAidBadTime_ms ; // time in msec gps aiding was last detected to be bad
float posDownAtTakeoff ; // flight vehicle vertical position sampled at transition from on-ground to in-air and used as a reference (m)
ftype posDownAtTakeoff ; // flight vehicle vertical position sampled at transition from on-ground to in-air and used as a reference (m)
bool useGpsVertVel ; // true if GPS vertical velocity should be used
float yawResetAngle ; // Change in yaw angle due to last in-flight yaw reset in radians. A positive value means the yaw angle has increased.
ftype yawResetAngle ; // Change in yaw angle due to last in-flight yaw reset in radians. A positive value means the yaw angle has increased.
uint32_t lastYawReset_ms ; // System time at which the last yaw reset occurred. Returned by getLastYawResetAngle
bool tiltAlignComplete ; // true when tilt alignment is complete
bool yawAlignComplete ; // true when yaw alignment is complete
@ -1038,14 +1044,14 @@ private:
@@ -1038,14 +1044,14 @@ private:
imu_elements imuDataDelayed ; // IMU data at the fusion time horizon
imu_elements imuDataNew ; // IMU data at the current time horizon
imu_elements imuDataDownSampledNew ; // IMU data at the current time horizon that has been downsampled to a 100Hz rate
Quaternion imuQuatDownSampleNew ; // Quaternion obtained by rotating through the IMU delta angles since the start of the current down sampled frame
QuaternionF imuQuatDownSampleNew ; // Quaternion obtained by rotating through the IMU delta angles since the start of the current down sampled frame
baro_elements baroDataNew ; // Baro data at the current time horizon
baro_elements baroDataDelayed ; // Baro data at the fusion time horizon
range_elements rangeDataNew ; // Range finder data at the current time horizon
range_elements rangeDataDelayed ; // Range finder data at the fusion time horizon
tas_elements tasDataNew ; // TAS data at the current time horizon
tas_elements tasDataDelayed ; // TAS data at the fusion time horizon
float tasErrVar ; // TAS error variance (m/s)**2
ftype tasErrVar ; // TAS error variance (m/s)**2
bool usingDefaultAirspeed ; // true when a default airspeed is being used instead of a measured value
mag_elements magDataDelayed ; // Magnetometer data at the fusion time horizon
gps_elements gpsDataNew ; // GPS data at the current time horizon
@ -1053,10 +1059,10 @@ private:
@@ -1053,10 +1059,10 @@ private:
uint8_t last_gps_idx ; // sensor ID of the GPS receiver used for the last fusion or reset
output_elements outputDataNew ; // output state data at the current time step
output_elements outputDataDelayed ; // output state data at the current time step
Vector3f delAngCorrection ; // correction applied to delta angles used by output observer to track the EKF
Vector3f velErrintegral ; // integral of output predictor NED velocity tracking error (m)
Vector3f posErrintegral ; // integral of output predictor NED position tracking error (m.sec)
float innovYaw ; // compass yaw angle innovation (rad)
Vector3F delAngCorrection ; // correction applied to delta angles used by output observer to track the EKF
Vector3F velErrintegral ; // integral of output predictor NED velocity tracking error (m)
Vector3F posErrintegral ; // integral of output predictor NED position tracking error (m.sec)
ftype innovYaw ; // compass yaw angle innovation (rad)
uint32_t timeTasReceived_ms ; // time last TAS data was received (msec)
bool gpsGoodToAlign ; // true when the GPS quality can be used to initialise the navigation system
uint32_t magYawResetTimer_ms ; // timer in msec used to track how long good magnetometer data is failing innovation consistency checks
@ -1068,94 +1074,94 @@ private:
@@ -1068,94 +1074,94 @@ private:
bool airSpdFusionDelayed ; // true when the air speed fusion has been delayed
bool sideSlipFusionDelayed ; // true when the sideslip fusion has been delayed
bool airDataFusionWindOnly ; // true when sideslip and airspeed fusion is only allowed to modify the wind states
Vector3f lastMagOffsets ; // Last magnetometer offsets from COMPASS_ parameters. Used to detect parameter changes.
Vector3F lastMagOffsets ; // Last magnetometer offsets from COMPASS_ parameters. Used to detect parameter changes.
bool lastMagOffsetsValid ; // True when lastMagOffsets has been initialized
Vector2f posResetNE ; // Change in North/East position due to last in-flight reset in metres. Returned by getLastPosNorthEastReset
Vector2F posResetNE ; // Change in North/East position due to last in-flight reset in metres. Returned by getLastPosNorthEastReset
uint32_t lastPosReset_ms ; // System time at which the last position reset occurred. Returned by getLastPosNorthEastReset
Vector2f velResetNE ; // Change in North/East velocity due to last in-flight reset in metres/sec. Returned by getLastVelNorthEastReset
Vector2F velResetNE ; // Change in North/East velocity due to last in-flight reset in metres/sec. Returned by getLastVelNorthEastReset
uint32_t lastVelReset_ms ; // System time at which the last velocity reset occurred. Returned by getLastVelNorthEastReset
float posResetD ; // Change in Down position due to last in-flight reset in metres. Returned by getLastPosDowntReset
ftype posResetD ; // Change in Down position due to last in-flight reset in metres. Returned by getLastPosDowntReset
uint32_t lastPosResetD_ms ; // System time at which the last position reset occurred. Returned by getLastPosDownReset
float yawTestRatio ; // square of magnetometer yaw angle innovation divided by fail threshold
Quaternion prevQuatMagReset ; // Quaternion from the last time the magnetic field state reset condition test was performed
float hgtInnovFiltState ; // state used for fitering of the height innovations used for pre-flight checks
ftype yawTestRatio ; // square of magnetometer yaw angle innovation divided by fail threshold
QuaternionF prevQuatMagReset ; // Quaternion from the last time the magnetic field state reset condition test was performed
ftype hgtInnovFiltState ; // state used for fitering of the height innovations used for pre-flight checks
uint8_t magSelectIndex ; // Index of the magnetometer that is being used by the EKF
bool runUpdates ; // boolean true when the EKF updates can be run
uint32_t framesSincePredict ; // number of frames lapsed since EKF instance did a state prediction
bool startPredictEnabled ; // boolean true when the frontend has given permission to start a new state prediciton cycle
uint8_t localFilterTimeStep_ms ; // average number of msec between filter updates
float posDownObsNoise ; // observation noise variance on the vertical position used by the state and covariance update step (m^2)
Vector3f delAngCorrected ; // corrected IMU delta angle vector at the EKF time horizon (rad)
Vector3f delVelCorrected ; // corrected IMU delta velocity vector at the EKF time horizon (m/s)
ftype posDownObsNoise ; // observation noise variance on the vertical position used by the state and covariance update step (m^2)
Vector3F delAngCorrected ; // corrected IMU delta angle vector at the EKF time horizon (rad)
Vector3F delVelCorrected ; // corrected IMU delta velocity vector at the EKF time horizon (m/s)
bool magFieldLearned ; // true when the magnetic field has been learned
uint32_t wasLearningCompass_ms ; // time when we were last waiting for compass learn to complete
Vector3f earthMagFieldVar ; // NED earth mag field variances for last learned field (mGauss^2)
Vector3f bodyMagFieldVar ; // XYZ body mag field variances for last learned field (mGauss^2)
Vector3F earthMagFieldVar ; // NED earth mag field variances for last learned field (mGauss^2)
Vector3F bodyMagFieldVar ; // XYZ body mag field variances for last learned field (mGauss^2)
bool delAngBiasLearned ; // true when the gyro bias has been learned
nav_filter_status filterStatus ; // contains the status of various filter outputs
float ekfOriginHgtVar ; // Variance of the EKF WGS-84 origin height estimate (m^2)
ftype ekfOriginHgtVar ; // Variance of the EKF WGS-84 origin height estimate (m^2)
double ekfGpsRefHgt ; // floating point representation of the WGS-84 reference height used to convert GPS height to local height (m)
uint32_t lastOriginHgtTime_ms ; // last time the ekf's WGS-84 origin height was corrected
Vector3f outputTrackError ; // attitude (rad), velocity (m/s) and position (m) tracking error magnitudes from the output observer
Vector3f velOffsetNED ; // This adds to the earth frame velocity estimate at the IMU to give the velocity at the body origin (m/s)
Vector3f posOffsetNED ; // This adds to the earth frame position estimate at the IMU to give the position at the body origin (m)
Vector3F outputTrackError ; // attitude (rad), velocity (m/s) and position (m) tracking error magnitudes from the output observer
Vector3F velOffsetNED ; // This adds to the earth frame velocity estimate at the IMU to give the velocity at the body origin (m/s)
Vector3F posOffsetNED ; // This adds to the earth frame position estimate at the IMU to give the position at the body origin (m)
uint32_t firstInitTime_ms ; // First time the initialise function was called (msec)
uint32_t lastInitFailReport_ms ; // Last time the buffer initialisation failure report was sent (msec)
float tiltErrorVariance ; // variance of the angular uncertainty measured perpendicular to the vertical (rad^2)
ftype tiltErrorVariance ; // variance of the angular uncertainty measured perpendicular to the vertical (rad^2)
// variables used to calculate a vertical velocity that is kinematically consistent with the vertical position
struct {
float pos ;
float vel ;
float acc ;
ftype pos ;
ftype vel ;
ftype acc ;
} vertCompFiltState ;
// variables used by the pre-initialisation GPS checks
struct Location gpsloc_prev ; // LLH location of previous GPS measurement
uint32_t lastPreAlignGpsCheckTime_ms ; // last time in msec the GPS quality was checked during pre alignment checks
float gpsDriftNE ; // amount of drift detected in the GPS position during pre-flight GPs checks
float gpsVertVelFilt ; // amount of filtered vertical GPS velocity detected during pre-flight GPS checks
float gpsHorizVelFilt ; // amount of filtered horizontal GPS velocity detected during pre-flight GPS checks
ftype gpsDriftNE ; // amount of drift detected in the GPS position during pre-flight GPs checks
ftype gpsVertVelFilt ; // amount of filtered vertical GPS velocity detected during pre-flight GPS checks
ftype gpsHorizVelFilt ; // amount of filtered horizontal GPS velocity detected during pre-flight GPS checks
// variable used by the in-flight GPS quality check
bool gpsSpdAccPass ; // true when reported GPS speed accuracy passes in-flight checks
bool ekfInnovationsPass ; // true when GPS innovations pass in-flight checks
float sAccFilterState1 ; // state variable for LPF applied to reported GPS speed accuracy
float sAccFilterState2 ; // state variable for peak hold filter applied to reported GPS speed
ftype sAccFilterState1 ; // state variable for LPF applied to reported GPS speed accuracy
ftype sAccFilterState2 ; // state variable for peak hold filter applied to reported GPS speed
uint32_t lastGpsCheckTime_ms ; // last time in msec the GPS quality was checked
uint32_t lastInnovPassTime_ms ; // last time in msec the GPS innovations passed
uint32_t lastInnovFailTime_ms ; // last time in msec the GPS innovations failed
bool gpsAccuracyGood ; // true when the GPS accuracy is considered to be good enough for safe flight.
Vector3f gpsVelInnov ; // gps velocity innovations
Vector3f gpsVelVarInnov ; // gps velocity innovation variances
Vector3F gpsVelInnov ; // gps velocity innovations
Vector3F gpsVelVarInnov ; // gps velocity innovation variances
uint32_t gpsVelInnovTime_ms ; // system time that gps velocity innovations were recorded (to detect timeouts)
// variables added for optical flow fusion
EKF_obs_buffer_t < of_elements > storedOF ; // OF data buffer
of_elements ofDataNew ; // OF data at the current time horizon
bool flowDataValid ; // true while optical flow data is still fresh
Vector2f auxFlowObsInnov ; // optical flow rate innovation from 1-state terrain offset estimator
Vector2F auxFlowObsInnov ; // optical flow rate innovation from 1-state terrain offset estimator
uint32_t flowValidMeaTime_ms ; // time stamp from latest valid flow measurement (msec)
uint32_t rngValidMeaTime_ms ; // time stamp from latest valid range measurement (msec)
uint32_t flowMeaTime_ms ; // time stamp from latest flow measurement (msec)
uint32_t gndHgtValidTime_ms ; // time stamp from last terrain offset state update (msec)
Matrix3f Tbn_flow ; // transformation matrix from body to nav axes at the middle of the optical flow sample period
Matrix3F Tbn_flow ; // transformation matrix from body to nav axes at the middle of the optical flow sample period
Vector2 varInnovOptFlow ; // optical flow innovations variances (rad/sec)^2
Vector2 innovOptFlow ; // optical flow LOS innovations (rad/sec)
float Popt ; // Optical flow terrain height state covariance (m^2)
float terrainState ; // terrain position state (m)
float prevPosN ; // north position at last measurement
float prevPosE ; // east position at last measurement
float varInnovRng ; // range finder observation innovation variance (m^2)
float innovRng ; // range finder observation innovation (m)
float hgtMea ; // height measurement derived from either baro, gps or range finder data (m)
ftype Popt ; // Optical flow terrain height state covariance (m^2)
ftype terrainState ; // terrain position state (m)
ftype prevPosN ; // north position at last measurement
ftype prevPosE ; // east position at last measurement
ftype varInnovRng ; // range finder observation innovation variance (m^2)
ftype innovRng ; // range finder observation innovation (m)
ftype hgtMea ; // height measurement derived from either baro, gps or range finder data (m)
bool inhibitGndState ; // true when the terrain position state is to remain constant
uint32_t prevFlowFuseTime_ms ; // time both flow measurement components passed their innovation consistency checks
Vector2 flowTestRatio ; // square of optical flow innovations divided by fail threshold used by main filter where >1.0 is a fail
Vector2f auxFlowTestRatio ; // sum of squares of optical flow innovation divided by fail threshold used by 1-state terrain offset estimator
float R_LOS ; // variance of optical flow rate measurements (rad/sec)^2
float auxRngTestRatio ; // square of range finder innovations divided by fail threshold used by main filter where >1.0 is a fail
Vector2f flowGyroBias ; // bias error of optical flow sensor gyro output
Vector2F auxFlowTestRatio ; // sum of squares of optical flow innovation divided by fail threshold used by 1-state terrain offset estimator
ftype R_LOS ; // variance of optical flow rate measurements (rad/sec)^2
ftype auxRngTestRatio ; // square of range finder innovations divided by fail threshold used by main filter where >1.0 is a fail
Vector2F flowGyroBias ; // bias error of optical flow sensor gyro output
bool rangeDataToFuse ; // true when valid range finder height data has arrived at the fusion time horizon.
bool baroDataToFuse ; // true when valid baro height finder data has arrived at the fusion time horizon.
bool gpsDataToFuse ; // true when valid GPS data has arrived at the fusion time horizon.
@ -1167,16 +1173,16 @@ private:
@@ -1167,16 +1173,16 @@ private:
AidingMode PV_AidingMode ; // Defines the preferred mode for aiding of velocity and position estimates from the INS
AidingMode PV_AidingModePrev ; // Value of PV_AidingMode from the previous frame - used to detect transitions
bool gndOffsetValid ; // true when the ground offset state can still be considered valid
Vector3f delAngBodyOF ; // bias corrected delta angle of the vehicle IMU measured summed across the time since the last OF measurement
float delTimeOF ; // time that delAngBodyOF is summed across
Vector3F delAngBodyOF ; // bias corrected delta angle of the vehicle IMU measured summed across the time since the last OF measurement
ftype delTimeOF ; // time that delAngBodyOF is summed across
bool flowFusionActive ; // true when optical flow fusion is active
Vector3f accelPosOffset ; // position of IMU accelerometer unit in body frame (m)
Vector3F accelPosOffset ; // position of IMU accelerometer unit in body frame (m)
// Range finder
float baroHgtOffset ; // offset applied when when switching to use of Baro height
float rngOnGnd ; // Expected range finder reading in metres when vehicle is on ground
float storedRngMeas [ 2 ] [ 3 ] ; // Ringbuffer of stored range measurements for dual range sensors
ftype baroHgtOffset ; // offset applied when when switching to use of Baro height
ftype rngOnGnd ; // Expected range finder reading in metres when vehicle is on ground
ftype storedRngMeas [ 2 ] [ 3 ] ; // Ringbuffer of stored range measurements for dual range sensors
uint32_t storedRngMeasTime_ms [ 2 ] [ 3 ] ; // Ringbuffers of stored range measurement times for dual range sensors
uint32_t lastRngMeasTime_ms ; // Timestamp of last range measurement
uint8_t rngMeasIndex [ 2 ] ; // Current range measurement ringbuffer index for dual range sensors
@ -1214,49 +1220,49 @@ private:
@@ -1214,49 +1220,49 @@ private:
EKF_obs_buffer_t < rng_bcn_elements > storedRangeBeacon ; // Beacon range buffer
rng_bcn_elements rngBcnDataDelayed ; // Range beacon data at the fusion time horizon
uint32_t lastRngBcnPassTime_ms ; // time stamp when the range beacon measurement last passed innovation consistency checks (msec)
float rngBcnTestRatio ; // Innovation test ratio for range beacon measurements
ftype rngBcnTestRatio ; // Innovation test ratio for range beacon measurements
bool rngBcnHealth ; // boolean true if range beacon measurements have passed innovation consistency check
float varInnovRngBcn ; // range beacon observation innovation variance (m^2)
float innovRngBcn ; // range beacon observation innovation (m)
ftype varInnovRngBcn ; // range beacon observation innovation variance (m^2)
ftype innovRngBcn ; // range beacon observation innovation (m)
uint32_t lastTimeRngBcn_ms [ 4 ] ; // last time we received a range beacon measurement (msec)
bool rngBcnDataToFuse ; // true when there is new range beacon data to fuse
Vector3f beaconVehiclePosNED ; // NED position estimate from the beacon system (NED)
float beaconVehiclePosErr ; // estimated position error from the beacon system (m)
Vector3F beaconVehiclePosNED ; // NED position estimate from the beacon system (NED)
ftype beaconVehiclePosErr ; // estimated position error from the beacon system (m)
uint32_t rngBcnLast3DmeasTime_ms ; // last time the beacon system returned a 3D fix (msec)
bool rngBcnGoodToAlign ; // true when the range beacon systems 3D fix can be used to align the filter
uint8_t lastRngBcnChecked ; // index of the last range beacon checked for data
Vector3f receiverPos ; // receiver NED position (m) - alignment 3 state filter
float receiverPosCov [ 3 ] [ 3 ] ; // Receiver position covariance (m^2) - alignment 3 state filter (
Vector3F receiverPos ; // receiver NED position (m) - alignment 3 state filter
ftype receiverPosCov [ 3 ] [ 3 ] ; // Receiver position covariance (m^2) - alignment 3 state filter (
bool rngBcnAlignmentStarted ; // True when the initial position alignment using range measurements has started
bool rngBcnAlignmentCompleted ; // True when the initial position alignment using range measurements has finished
uint8_t lastBeaconIndex ; // Range beacon index last read - used during initialisation of the 3-state filter
Vector3f rngBcnPosSum ; // Sum of range beacon NED position (m) - used during initialisation of the 3-state filter
Vector3F rngBcnPosSum ; // Sum of range beacon NED position (m) - used during initialisation of the 3-state filter
uint8_t numBcnMeas ; // Number of beacon measurements - used during initialisation of the 3-state filter
float rngSum ; // Sum of range measurements (m) - used during initialisation of the 3-state filter
ftype rngSum ; // Sum of range measurements (m) - used during initialisation of the 3-state filter
uint8_t N_beacons ; // Number of range beacons in use
float maxBcnPosD ; // maximum position of all beacons in the down direction (m)
float minBcnPosD ; // minimum position of all beacons in the down direction (m)
ftype maxBcnPosD ; // maximum position of all beacons in the down direction (m)
ftype minBcnPosD ; // minimum position of all beacons in the down direction (m)
bool usingMinHypothesis ; // true when the min beacon constellation offset hypothesis is being used
float bcnPosDownOffsetMax ; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
float bcnPosOffsetMaxVar ; // Variance of the bcnPosDownOffsetMax state (m)
float maxOffsetStateChangeFilt ; // Filtered magnitude of the change in bcnPosOffsetHigh
ftype bcnPosDownOffsetMax ; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
ftype bcnPosOffsetMaxVar ; // Variance of the bcnPosDownOffsetMax state (m)
ftype maxOffsetStateChangeFilt ; // Filtered magnitude of the change in bcnPosOffsetHigh
float bcnPosDownOffsetMin ; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
float bcnPosOffsetMinVar ; // Variance of the bcnPosDownOffsetMin state (m)
float minOffsetStateChangeFilt ; // Filtered magnitude of the change in bcnPosOffsetLow
ftype bcnPosDownOffsetMin ; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
ftype bcnPosOffsetMinVar ; // Variance of the bcnPosDownOffsetMin state (m)
ftype minOffsetStateChangeFilt ; // Filtered magnitude of the change in bcnPosOffsetLow
Vector3f bcnPosOffsetNED ; // NED position of the beacon origin in earth frame (m)
Vector3F bcnPosOffsetNED ; // NED position of the beacon origin in earth frame (m)
bool bcnOriginEstInit ; // True when the beacon origin has been initialised
// Range Beacon Fusion Debug Reporting
uint8_t rngBcnFuseDataReportIndex ; // index of range beacon fusion data last reported
struct rngBcnFusionReport_t {
float rng ; // measured range to beacon (m)
float innov ; // range innovation (m)
float innovVar ; // innovation variance (m^2)
float testRatio ; // innovation consistency test ratio
Vector3f beaconPosNED ; // beacon NED position
ftype rng ; // measured range to beacon (m)
ftype innov ; // range innovation (m)
ftype innovVar ; // innovation variance (m^2)
ftype testRatio ; // innovation consistency test ratio
Vector3F beaconPosNED ; // beacon NED position
} * rngBcnFusionReport ;
# if EK3_FEATURE_DRAG_FUSION
@ -1265,10 +1271,10 @@ private:
@@ -1265,10 +1271,10 @@ private:
drag_elements dragSampleDelayed ;
drag_elements dragDownSampled ; // down sampled from filter prediction rate to observation rate
uint8_t dragSampleCount ; // number of drag specific force samples accumulated at the filter prediction rate
float dragSampleTimeDelta ; // time integral across all samples used to form _drag_down_sampled (sec)
Vector2f innovDrag ; // multirotor drag measurement innovation (m/sec**2)
Vector2f innovDragVar ; // multirotor drag measurement innovation variance ((m/sec**2)**2)
Vector2f dragTestRatio ; // drag innovation consistency check ratio
ftype dragSampleTimeDelta ; // time integral across all samples used to form _drag_down_sampled (sec)
Vector2F innovDrag ; // multirotor drag measurement innovation (m/sec**2)
Vector2F innovDragVar ; // multirotor drag measurement innovation variance ((m/sec**2)**2)
Vector2F dragTestRatio ; // drag innovation consistency check ratio
# endif
bool dragFusionEnabled ;
@ -1278,11 +1284,11 @@ private:
@@ -1278,11 +1284,11 @@ private:
// Movement detector
bool takeOffDetected ; // true when takeoff for optical flow navigation has been detected
float rngAtStartOfFlight ; // range finder measurement at start of flight
ftype rngAtStartOfFlight ; // range finder measurement at start of flight
uint32_t timeAtArming_ms ; // time in msec that the vehicle armed
// baro ground effect
float meaHgtAtTakeOff ; // height measured at commencement of takeoff
ftype meaHgtAtTakeOff ; // height measured at commencement of takeoff
// control of post takeoff magnetic field and heading resets
bool finalInflightYawInit ; // true when the final post takeoff initialisation of yaw angle has been performed
@ -1291,22 +1297,22 @@ private:
@@ -1291,22 +1297,22 @@ private:
bool magStateResetRequest ; // true if magnetic field states need to be reset using the magnetomter measurements
bool magYawResetRequest ; // true if the vehicle yaw and magnetic field states need to be reset using the magnetometer measurements
bool gpsYawResetRequest ; // true if the vehicle yaw needs to be reset to the GPS course
float posDownAtLastMagReset ; // vertical position last time the mag states were reset (m)
float yawInnovAtLastMagReset ; // magnetic yaw innovation last time the yaw and mag field states were reset (rad)
Quaternion quatAtLastMagReset ; // quaternion states last time the mag states were reset
ftype posDownAtLastMagReset ; // vertical position last time the mag states were reset (m)
ftype yawInnovAtLastMagReset ; // magnetic yaw innovation last time the yaw and mag field states were reset (rad)
QuaternionF quatAtLastMagReset ; // quaternion states last time the mag states were reset
// Used by on ground movement check required when operating on ground without a yaw reference
float gyro_diff ; // filtered gyro difference (rad/s)
float accel_diff ; // filtered acceerometer difference (m/s/s)
Vector3f gyro_prev ; // gyro vector from previous time step (rad/s)
Vector3f accel_prev ; // accelerometer vector from previous time step (m/s/s)
ftype gyro_diff ; // filtered gyro difference (rad/s)
ftype accel_diff ; // filtered acceerometer difference (m/s/s)
Vector3F gyro_prev ; // gyro vector from previous time step (rad/s)
Vector3F accel_prev ; // accelerometer vector from previous time step (m/s/s)
bool onGroundNotMoving ; // true when on the ground and not moving
uint32_t lastMoveCheckLogTime_ms ; // last time the movement check data was logged (msec)
// variables used to inhibit accel bias learning
bool inhibitDelVelBiasStates ; // true when all IMU delta velocity bias states are de-activated
bool dvelBiasAxisInhibit [ 3 ] { } ; // true when IMU delta velocity bias states for a specific axis is de-activated
Vector3f dvelBiasAxisVarPrev ; // saved delta velocity XYZ bias variances (m/sec)**2
Vector3F dvelBiasAxisVarPrev ; // saved delta velocity XYZ bias variances (m/sec)**2
# if EK3_FEATURE_EXTERNAL_NAV
// external navigation fusion
@ -1320,8 +1326,8 @@ private:
@@ -1320,8 +1326,8 @@ private:
ext_nav_vel_elements extNavVelDelayed ; // external navigation velocity data at the fusion time horizon. Already corrected for sensor position
uint32_t extNavVelMeasTime_ms ; // time external navigation velocity measurements were accepted for input to the data buffer (msec)
bool extNavVelToFuse ; // true when there is new external navigation velocity to fuse
Vector3f extNavVelInnov ; // external nav velocity innovations
Vector3f extNavVelVarInnov ; // external nav velocity innovation variances
Vector3F extNavVelInnov ; // external nav velocity innovations
Vector3F extNavVelVarInnov ; // external nav velocity innovation variances
uint32_t extNavVelInnovTime_ms ; // system time that external nav velocity innovations were recorded (to detect timeouts)
EKF_obs_buffer_t < yaw_elements > storedExtNavYawAng ; // external navigation yaw angle buffer
yaw_elements extNavYawAngDataDelayed ; // external navigation yaw angle at the fusion time horizon
@ -1384,8 +1390,8 @@ private:
@@ -1384,8 +1390,8 @@ private:
ftype magXbias ;
ftype magYbias ;
ftype magZbias ;
Matrix3f DCM ;
Vector3f MagPred ;
Matrix3F DCM ;
Vector3F MagPred ;
ftype R_MAG ;
Vector9 SH_MAG ;
} mag_state ;
@ -1395,8 +1401,8 @@ private:
@@ -1395,8 +1401,8 @@ private:
// earth field from WMM tables
bool have_table_earth_field ; // true when we have initialised table_earth_field_ga
Vector3f table_earth_field_ga ; // earth field from WMM tables
float table_declination ; // declination in radians from the tables
Vector3F table_earth_field_ga ; // earth field from WMM tables
ftype table_declination ; // declination in radians from the tables
// timing statistics
struct ekf_timing timing ;
@ -1408,7 +1414,7 @@ private:
@@ -1408,7 +1414,7 @@ private:
bool assume_zero_sideslip ( void ) const ;
// vehicle specific initial gyro bias uncertainty
float InitialGyroBiasUncertainty ( void ) const ;
ftype InitialGyroBiasUncertainty ( void ) const ;
/*
learn magnetometer biases from GPS yaw . Return true if the
Andrew Tridgell
4 years ago
committed by
Randy Mackay