@ -63,6 +63,13 @@
@@ -63,6 +63,13 @@
// number of seconds a request to reset the yaw to the GSF estimate is active before it times out
# define YAW_RESET_TO_GSF_TIMEOUT_MS 5000
// limit on horizontal position states
# if HAL_WITH_EKF_DOUBLE
# define EK2_POSXY_STATE_LIMIT 50.0e6
# else
# define EK2_POSXY_STATE_LIMIT 1.0e6
# endif
class AP_AHRS ;
class NavEKF2_core : public NavEKF_core_common
@ -87,7 +94,7 @@ public:
@@ -87,7 +94,7 @@ public:
// Return a consolidated error score where higher numbers are less healthy
// Intended to be used by the front-end to determine which is the primary EKF
float errorScore ( void ) const ;
ftype errorScore ( void ) const ;
// Write the last calculated NE position relative to the reference point (m).
// If a calculated solution is not available, use the best available data and return false
@ -383,16 +390,16 @@ private:
@@ -383,16 +390,16 @@ private:
// broken down as individual elements. Both are equivalent (same
// memory)
struct state_elements {
Vector3f angErr ; // 0..2
Vector3f velocity ; // 3..5
Vector3f position ; // 6..8
Vector3f gyro_bias ; // 9..11
Vector3f gyro_scale ; // 12..14
float accel_zbias ; // 15
Vector3f earth_magfield ; // 16..18
Vector3f body_magfield ; // 19..21
Vector2f wind_vel ; // 22..23
Quaternion quat ; // 24..27
Vector3F angErr ; // 0..2
Vector3F velocity ; // 3..5
Vector3F position ; // 6..8
Vector3F gyro_bias ; // 9..11
Vector3F gyro_scale ; // 12..14
ftype accel_zbias ; // 15
Vector3F earth_magfield ; // 16..18
Vector3F body_magfield ; // 19..21
Vector2F wind_vel ; // 22..23
QuaternionF quat ; // 24..27
} ;
union {
@ -401,78 +408,78 @@ private:
@@ -401,78 +408,78 @@ private:
} ;
struct output_elements {
Quaternion quat ; // 0..3
Vector3f velocity ; // 4..6
Vector3f position ; // 7..9
QuaternionF quat ; // 0..3
Vector3F velocity ; // 4..6
Vector3F position ; // 7..9
} ;
struct imu_elements {
Vector3f delAng ; // 0..2
Vector3f delVel ; // 3..5
float delAngDT ; // 6
float delVelDT ; // 7
Vector3F delAng ; // 0..2
Vector3F delVel ; // 3..5
ftype delAngDT ; // 6
ftype delVelDT ; // 7
uint32_t time_ms ; // 8
uint8_t gyro_index ;
uint8_t accel_index ;
} ;
struct gps_elements : EKF_obs_element_t {
Vector2f pos ;
float hgt ;
Vector3f vel ;
Vector2F pos ;
ftype hgt ;
Vector3F vel ;
uint8_t sensor_idx ;
} ;
struct mag_elements : EKF_obs_element_t {
Vector3f mag ;
Vector3F mag ;
} ;
struct baro_elements : EKF_obs_element_t {
float hgt ;
ftype hgt ;
} ;
struct range_elements : EKF_obs_element_t {
float rng ;
ftype rng ;
uint8_t sensor_idx ;
} ;
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 ;
ftype tas ;
} ;
struct of_elements : EKF_obs_element_t {
Vector2f flowRadXY ;
Vector2f flowRadXYcomp ;
Vector3f bodyRadXYZ ;
Vector3f body_offset ;
Vector2F flowRadXY ;
Vector2F flowRadXYcomp ;
Vector3F bodyRadXYZ ;
Vector3F body_offset ;
} ;
struct ext_nav_elements : EKF_obs_element_t {
Vector3f pos ; // XYZ position measured in a RH navigation frame (m)
Quaternion quat ; // quaternion describing the rotation from navigation to body frame
float posErr ; // spherical poition measurement error 1-std (m)
float angErr ; // spherical angular measurement error 1-std (rad)
Vector3F pos ; // XYZ position measured in a RH navigation frame (m)
QuaternionF quat ; // quaternion describing the rotation from navigation to body frame
ftype posErr ; // spherical poition measurement error 1-std (m)
ftype angErr ; // spherical angular measurement error 1-std (rad)
bool posReset ; // true when the position measurement has been reset
} ;
// bias estimates for the IMUs that are enabled but not being used
// by this core.
struct {
Vector3f gyro_bias ;
Vector3f gyro_scale ;
float accel_zbias ;
Vector3F gyro_bias ;
Vector3F gyro_scale ;
ftype accel_zbias ;
} inactiveBias [ INS_MAX_INSTANCES ] ;
struct ext_nav_vel_elements : EKF_obs_element_t {
Vector3f vel ; // velocity in NED (m)
float err ; // velocity measurement error (m/s)
Vector3F vel ; // velocity in NED (m)
ftype err ; // velocity measurement error (m/s)
} ;
// update the navigation filter status
@ -509,7 +516,7 @@ private:
@@ -509,7 +516,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 ( ) ;
@ -533,21 +540,21 @@ private:
@@ -533,21 +540,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 ( ) ;
@ -593,7 +600,7 @@ private:
@@ -593,7 +600,7 @@ private:
// initialise the earth magnetic field states using declination and current attitude and magnetometer measurements
// and return attitude quaternion
Quaternion calcQuatAndFieldStates ( float roll , float pitch ) ;
QuaternionF calcQuatAndFieldStates ( ftype roll , ftype pitch ) ;
// zero stored variables
void InitialiseVariables ( ) ;
@ -604,7 +611,7 @@ private:
@@ -604,7 +611,7 @@ private:
void ResetPosition ( void ) ;
// reset the stateStruct's NE position to the specified position
void ResetPositionNE ( float posN , float posE ) ;
void ResetPositionNE ( ftype posN , ftype posE ) ;
// reset velocity states using the last GPS measurement
void ResetVelocity ( void ) ;
@ -613,7 +620,7 @@ private:
@@ -613,7 +620,7 @@ private:
void ResetHeight ( void ) ;
// reset the stateStruct's D position
void ResetPositionD ( float posD ) ;
void ResetPositionD ( ftype posD ) ;
// return true if we should use the airspeed sensor
bool useAirspeed ( void ) const ;
@ -631,7 +638,7 @@ private:
@@ -631,7 +638,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 optical flow data is available
bool optFlowDataPresent ( void ) const ;
@ -697,10 +704,10 @@ private:
@@ -697,10 +704,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
@ -734,10 +741,10 @@ private:
@@ -734,10 +741,10 @@ private:
void CorrectGPSForAntennaOffset ( gps_elements & gps_data ) const ;
// correct external navigation earth-frame position using sensor body-frame offset
void CorrectExtNavForSensorOffset ( Vector3f & ext_position ) const ;
void CorrectExtNavForSensorOffset ( Vector3F & ext_position ) const ;
// correct external navigation earth-frame velocity using sensor body-frame offset
void CorrectExtNavVelForSensorOffset ( Vector3f & ext_velocity ) const ;
void CorrectExtNavVelForSensorOffset ( Vector3F & ext_velocity ) const ;
// Runs the IMU prediction step for an independent GSF yaw estimator algorithm
// that uses IMU, GPS horizontal velocity and optionally true airspeed data.
@ -753,7 +760,7 @@ private:
@@ -753,7 +760,7 @@ private:
// yaw : new yaw angle (rad)
// yaw_variance : variance of new yaw angle (rad^2)
// isDeltaYaw : true when the yaw should be added to the existing yaw angle
void resetQuatStateYawOnly ( float yaw , float yawVariance , bool isDeltaYaw ) ;
void resetQuatStateYawOnly ( ftype yaw , ftype yawVariance , bool isDeltaYaw ) ;
// attempt to reset the yaw to the EKF-GSF value
// returns false if unsuccessful
@ -775,7 +782,7 @@ private:
@@ -775,7 +782,7 @@ private:
bool tasTimeout ; // boolean true if true airspeed measurements have failed for too long and have timed out
bool badIMUdata ; // boolean true if the bad IMU data is detected
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
@ -784,10 +791,10 @@ private:
@@ -784,10 +791,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)
@ -804,8 +811,8 @@ private:
@@ -804,8 +811,8 @@ 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
bool magFusePerformed ; // boolean set to true when magnetometer fusion has been performed in that time step
@ -813,8 +820,8 @@ private:
@@ -813,8 +820,8 @@ private:
uint32_t prevBetaStep_ms ; // time stamp of last synthetic sideslip fusion step
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
@ -829,13 +836,13 @@ private:
@@ -829,13 +836,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 lastYawTime_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
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
float defaultAirSpeed ; // default equivalent airspeed in m/s to be used if the measurement is unavailable. Do not use if not positive.
Vector2F lastKnownPositionNE ; // last known position
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
ftype defaultAirSpeed ; // default equivalent airspeed in m/s to be used if the measurement is unavailable. Do not use if not positive.
bool inhibitWindStates ; // true when wind states and covariances are to remain constant
bool inhibitMagStates ; // true when magnetic field states and covariances are to remain constant
bool lastInhibitMagStates ; // previous inhibitMagStates
@ -844,18 +851,18 @@ private:
@@ -844,18 +851,18 @@ private:
uint8_t last_gps_idx ; // sensor ID of the GPS receiver used for the last fusion or reset
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
Vector3f tiltErrVec ; // Vector of most recent attitude error correction from Vel,Pos fusion
float tiltErrFilt ; // Filtered tilt error metric
Vector3F tiltErrVec ; // Vector of most recent attitude error correction from Vel,Pos fusion
ftype tiltErrFilt ; // Filtered tilt error metric
bool tiltAlignComplete ; // true when tilt alignment is complete
bool yawAlignComplete ; // true when yaw alignment is complete
bool magStateInitComplete ; // true when the magnetic field sttes have been initialised
@ -863,7 +870,7 @@ private:
@@ -863,7 +870,7 @@ 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
@ -875,10 +882,10 @@ private:
@@ -875,10 +882,10 @@ private:
gps_elements gpsDataDelayed ; // GPS data at the fusion time horizon
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
@ -889,57 +896,57 @@ private:
@@ -889,57 +896,57 @@ private:
bool optFlowFusionDelayed ; // true when the optical flow fusion has been delayed
bool airSpdFusionDelayed ; // true when the air speed fusion has been delayed
bool sideSlipFusionDelayed ; // true when the sideslip fusion has been delayed
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 cycele
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)
// variables used to calculate a vertical velocity that is kinematically consistent with the verical 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 filterred vertical GPS velocity detected durng 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 filterred vertical GPS velocity detected durng 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 applid to reported GPS speed accuracy
float sAccFilterState2 ; // state variable for peak hold filter applied to reported GPS speed
ftype sAccFilterState1 ; // state variable for LPF applid 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
@ -951,28 +958,28 @@ private:
@@ -951,28 +958,28 @@ private:
of_elements ofDataDelayed ; // OF data at the fusion time horizon
bool flowDataToFuse ; // true when optical flow data is ready for fusion
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.
@ -984,15 +991,15 @@ private:
@@ -984,15 +991,15 @@ 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 accelPosOffset ; // position of IMU accelerometer unit in body frame (m)
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
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
@ -1003,47 +1010,47 @@ private:
@@ -1003,47 +1010,47 @@ private:
rng_bcn_elements rngBcnDataNew ; // Range beacon data at the current time horizon
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 innvovation 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
bool rngBcnTimeout ; // boolean true if range beacon measurements have faled innovation consistency checks for too long
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 [ 10 ] ; // 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)
float bcnPosOffset ; // Vertical position offset of the beacon constellation origin relative to the EKF origin (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)
ftype bcnPosOffset ; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
float bcnPosOffsetMax ; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
float bcnPosOffsetMaxVar ; // Variance of the bcnPosOffsetHigh state (m)
float OffsetMaxInnovFilt ; // Filtered magnitude of the range innovations using bcnPosOffsetHigh
ftype bcnPosOffsetMax ; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
ftype bcnPosOffsetMaxVar ; // Variance of the bcnPosOffsetHigh state (m)
ftype OffsetMaxInnovFilt ; // Filtered magnitude of the range innovations using bcnPosOffsetHigh
float bcnPosOffsetMin ; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
float bcnPosOffsetMinVar ; // Variance of the bcnPosoffset state (m)
float OffsetMinInnovFilt ; // Filtered magnitude of the range innovations using bcnPosOffsetLow
ftype bcnPosOffsetMin ; // Vertical position offset of the beacon constellation origin relative to the EKF origin (m)
ftype bcnPosOffsetMinVar ; // Variance of the bcnPosoffset state (m)
ftype OffsetMinInnovFilt ; // Filtered magnitude of the range innovations using bcnPosOffsetLow
// 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 [ 10 ] ;
// height source selection logic
@ -1051,11 +1058,11 @@ private:
@@ -1051,11 +1058,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
@ -1063,9 +1070,9 @@ private:
@@ -1063,9 +1070,9 @@ private:
bool magStateResetRequest ; // true if magnetic field states need to be reset using the magneteomter 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
uint8_t magYawAnomallyCount ; // Number of times the yaw has been reset due to a magnetic anomaly during initial ascent
// external navigation fusion
@ -1136,8 +1143,8 @@ private:
@@ -1136,8 +1143,8 @@ private:
ftype magXbias ;
ftype magYbias ;
ftype magZbias ;
Matrix3f DCM ;
Vector3f MagPred ;
Matrix3F DCM ;
Vector3F MagPred ;
ftype R_MAG ;
Vector9 SH_MAG ;
} mag_state ;
@ -1147,8 +1154,8 @@ private:
@@ -1147,8 +1154,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 ;
@ -1160,7 +1167,7 @@ private:
@@ -1160,7 +1167,7 @@ private:
bool assume_zero_sideslip ( void ) const ;
// vehicle specific initial gyro bias uncertainty
float InitialGyroBiasUncertainty ( void ) const ;
ftype InitialGyroBiasUncertainty ( void ) const ;
// The following declarations are used to control when the main navigation filter resets it's yaw to the estimate provided by the GSF
uint32_t EKFGSF_yaw_reset_ms ; // timestamp of last emergency yaw reset (uSec)
Andrew Tridgell
4 years ago
committed by
Randy Mackay