uint32_timuSampleTime_ms;// time that the last IMU value was taken
boolnewDataMag;// true when new magnetometer data has arrived
boolnewDataTas;// true when new airspeed data has arrived
booltasDataWaiting;// true when new airspeed data is waiting to be fused
uint32_tlastHgtReceived_ms;// time last time we received height data
uint16_thgtRetryTime;// time allowed without use of height measurements before a height timeout is declared
uint32_tlastVelPassTime;// time stamp when GPS velocity measurement last passed innovation consistency check (msec)
uint32_tlastPosPassTime;// time stamp when GPS position measurement last passed innovation consistency check (msec)
uint32_tlastPosFailTime;// time stamp when GPS position measurement last failed innovation consistency check (msec)
uint32_tlastHgtPassTime;// time stamp when height measurement last passed innovation consistency check (msec)
uint32_tlastTasPassTime;// time stamp when airspeed measurement last passed innovation consistency check (msec)
uint32_tlastStateStoreTime_ms;// time of last state vector storage
uint32_tlastTimeGpsReceived_ms;// last time we recieved GPS data
uint32_ttimeAtLastAuxEKF_ms;// last time the auxilliary filter was run to fuse range or optical flow measurements
uint32_tsecondLastGpsTime_ms;// time of second last GPS fix used to determine how long since last update
uint32_tlastHealthyMagTime_ms;// time the magnetometer was last declared healthy
uint32_tekfStartTime_ms;// time the EKF was started (msec)
Vector3flastAngRate;// angular rate from previous IMU sample used for trapezoidal integrator
Vector3flastAccel1;// acceleration from previous IMU1 sample used for trapezoidal integrator
Vector3flastAccel2;// acceleration from previous IMU2 sample used for trapezoidal integrator
Matrix24nextP;// Predicted covariance matrix before addition of process noise to diagonals
Vector24processNoise;// process noise added to diagonals of predicted covariance matrix
Vector25SF;// intermediate variables used to calculate predicted covariance matrix
Vector5SG;// intermediate variables used to calculate predicted covariance matrix
Vector8SQ;// intermediate variables used to calculate predicted covariance matrix
Vector23SPP;// intermediate variables used to calculate predicted covariance matrix
floatIMU1_weighting;// Weighting applied to use of IMU1. Varies between 0 and 1.
boolyawAligned;// true when the yaw angle has been aligned
Vector2fgpsPosGlitchOffsetNE;// offset applied to GPS data in the NE direction to compensate for rapid changes in GPS solution
Vector2flastKnownPositionNE;// last known position
uint32_tlastDecayTime_ms;// time of last decay of GPS position offset
floatvelTestRatio;// sum of squares of GPS velocity innovation divided by fail threshold
floatposTestRatio;// sum of squares of GPS position innovation divided by fail threshold
floathgtTestRatio;// sum of squares of baro height innovation divided by fail threshold
Vector3fmagTestRatio;// sum of squares of magnetometer innovations divided by fail threshold
floattasTestRatio;// sum of squares of true airspeed innovation divided by fail threshold
boolinhibitWindStates;// true when wind states and covariances are to remain constant
boolinhibitMagStates;// true when magnetic field states and covariances are to remain constant
boolfirstArmComplete;// true when first transition out of static mode has been performed after start up
boolfirstMagYawInit;// true when the first post takeoff initialisation of earth field and yaw angle has been performed
boolsecondMagYawInit;// true when the second post takeoff initialisation of earth field and yaw angle has been performed
boolflowTimeout;// true when optical flow measurements have time out
Vector2fgpsVelGlitchOffset;// Offset applied to the GPS velocity when the gltch radius is being decayed back to zero
boolgpsNotAvailable;// bool true when valid GPS data is not available
boolfilterArmed;// true when the vehicle is disarmed
boolprevFilterArmed;// vehicleArmed from previous frame
structLocationEKF_origin;// LLH origin of the NED axis system - do not change unless filter is reset
boolvalidOrigin;// true when the EKF origin is valid
floatgpsSpdAccuracy;// estimated speed accuracy in m/s returned by the UBlox GPS receiver
uint32_tlastGpsVelFail_ms;// time of last GPS vertical velocity consistency check fail
Vector3flastMagOffsets;// magnetometer offsets returned by compass object from previous update
boolgpsAidingBad;// true when GPS position measurements have been consistently rejected by the filter
uint32_tlastGpsAidBadTime_ms;// time in msec gps aiding was last detected to be bad
floatposDownAtArming;// flight vehicle vertical position at arming used as a reference point
boolhighYawRate;// true when the vehicle is doing rapid yaw rotation where gyro scel factor errors could cause loss of heading reference
floatyawRateFilt;// filtered yaw rate used to determine when the vehicle is doing rapid yaw rotation where gyro scel factor errors could cause loss of heading reference
booluseGpsVertVel;// true if GPS vertical velocity should be used
floatyawResetAngle;// Change in yaw angle due to last in-flight yaw reset in radians. A positive value means the yaw angle has increased.
boolyawResetAngleWaiting;// true when the yaw reset angle has been updated and has not been retrieved via the getLastYawResetAngle() function
Vector3ftiltErrVec;// Vector of most recent attitude error correction from Vel,Pos fusion
floattiltErrFilt;// Filtered tilt error metric
booltiltAlignComplete;// true when tilt alignment is complete
boolyawAlignComplete;// true when yaw alignment is complete
uint8_tstateIndexLim;// Max state index used during matrix and array operations
imu_elementsimuDataDelayed;// IMU data at the fusion time horizon
imu_elementsimuDataNew;// IMU data at the current time horizon
uint8_tfifoIndexNow;// Global index for inertial and output solution at current time horizon
uint8_tfifoIndexDelayed;// Global index for inertial and output solution at delayed/fusion time horizon
uint32_thgtMeasTime_ms;// Effective measurement time of last received height measurement
uint32_tmagMeasTime_ms;// Effective measurement time of last received magnetometer measurement
baro_elementsbaroDataNew;// Baro data at the current time horizon
baro_elementsbaroDataDelayed;// Baro data at the fusion time horizon
uint8_tbaroStoreIndex;// Baro data storage index
tas_elementstasDataNew;// TAS data at the current time horizon
tas_elementstasDataDelayed;// TAS data at the fusion time horizon
uint8_ttasStoreIndex;// TAS data storage index
mag_elementsmagDataNew;// Magnetometer data at the current time horizon
mag_elementsmagDataDelayed;// Magnetometer data at the fusion time horizon
uint8_tmagStoreIndex;// Magnetometer data storage index
gps_elementsgpsDataNew;// GPS data at the current time horizon
gps_elementsgpsDataDelayed;// GPS data at the fusion time horizon
uint8_tgpsStoreIndex;// GPS data storage index
output_elementsoutputDataNew;// output state data at the current time step
output_elementsoutputDataDelayed;// output state data at the current time step
Vector3fdelAngCorrection;// correction applied to delta angles used by output observer to track the EKF
Vector3fdelVelCorrection;// correction applied to earth frame delta velocities used by output observer to track the EKF
Vector3fvelCorrection;// correction applied to velocities used by the output observer to track the EKF
Vector2innovOptFlow;// optical flow LOS innovations (rad/sec)
floatPopt;// Optical flow terrain height state covariance (m^2)
floatterrainState;// terrain position state (m)
floatprevPosN;// north position at last measurement
floatprevPosE;// east position at last measurement
boolfuseRngData;// true when fusion of range data is demanded
floatvarInnovRng;// range finder observation innovation variance (m^2)
floatinnovRng;// range finder observation innovation (m)
floatrngMea;// range finder measurement (m)
boolinhibitGndState;// true when the terrain position state is to remain constant
uint32_tprevFlowFuseTime_ms;// time both flow measurement components passed their innovation consistency checks
Vector2flowTestRatio;// square of optical flow innovations divided by fail threshold used by main filter where >1.0 is a fail
floatauxFlowTestRatio;// sum of squares of optical flow innovation divided by fail threshold used by 1-state terrain offset estimator
floatR_LOS;// variance of optical flow rate measurements (rad/sec)^2
floatauxRngTestRatio;// square of range finder innovations divided by fail threshold used by main filter where >1.0 is a fail
Vector2fflowGyroBias;// bias error of optical flow sensor gyro output
boolnewDataRng;// true when new valid range finder data has arrived.
boolconstVelMode;// true when fusing a constant velocity to maintain attitude reference when either optical flow or GPS measurements are lost after arming
boollastConstVelMode;// last value of holdVelocity
Vector2fheldVelNE;// velocity held when no aiding is available
enumAidingMode{AID_ABSOLUTE=0,// GPS aiding is being used (optical flow may also be used) so position estimates are absolute.
AID_NONE=1,// no aiding is being used so only attitude and height estimates are available. Either constVelMode or constPosMode must be used to constrain tilt drift.
AID_RELATIVE=2// only optical flow aiding is being used so position estimates will be relative
};
AidingModePV_AidingMode;// Defines the preferred mode for aiding of velocity and position estimates from the INS
boolgndOffsetValid;// true when the ground offset state can still be considered valid
boolflowXfailed;// true when the X optical flow measurement has failed the innovation consistency check
Vector3fdelAngBodyOF;// bias corrected delta angle of the vehicle IMU measured summed across the time since the last OF measurement
floatdelTimeOF;// time that delAngBodyOF is summed across
// Range finder
floatbaroHgtOffset;// offset applied when baro height used as a backup height reference if range-finder fails
floatrngOnGnd;// Expected range finder reading in metres when vehicle is on ground
floatstoredRngMeas[3];// Ringbuffer of stored range measurements
uint32_tstoredRngMeasTime_ms[3];// Ringbuffer of stored range measurement times
uint32_tlastRngMeasTime_ms;// Timestamp of last range measurement
uint8_trngMeasIndex;// Current range measurement ringbuffer index
// Movement detector
booltakeOffDetected;// true when takeoff for optical flow navigation has been detected
floatrangeAtArming;// range finder measurement when armed
uint32_ttimeAtArming_ms;// time in msec that the vehicle armed
// IMU processing
floatdtDelVel;
floatdtDelVel2;
// baro ground effect
boolexpectGndEffectTakeoff;// external state from ArduCopter - takeoff expected
uint32_ttakeoffExpectedSet_ms;// system time at which expectGndEffectTakeoff was set
boolexpectGndEffectTouchdown;// external state from ArduCopter - touchdown expected
uint32_ttouchdownExpectedSet_ms;// system time at which expectGndEffectTouchdown was set
floatmeaHgtAtTakeOff;// height measured at commencement of takeoff
struct{
boolbad_xmag:1;
boolbad_ymag:1;
boolbad_zmag:1;
boolbad_airspeed:1;
boolbad_sideslip:1;
}faultStatus;
// states held by magnetomter fusion across time steps
// magnetometer X,Y,Z measurements are fused across three time steps
// to level computational load as this is an expensive operation
