boolvelHealth;// boolean true if velocity measurements have failed innovation consistency check
boolposHealth;// boolean true if position measurements have failed innovation consistency check
boolhgtHealth;// boolean true if height measurements have failed innovation consistency check
boolvelTimeout;// boolean true if velocity measurements have failed innovation consistency check and timed out
boolposTimeout;// boolean true if position measurements have failed innovation consistency check and timed out
boolhgtTimeout;// boolean true if height measurements have failed innovation consistency check and timed out
Vector22states;// state matrix - 4 x quaternions, 3 x Vel, 3 x Pos, 3 x gyro bias, 3 x accel bias, 2 x wind vel, 3 x earth mag field, 3 x body mag field
Vector22Kfusion;// Kalman gain vector
Matrix22KH;// intermediate result used for covariance updates
Matrix22KHP;// intermediate result used for covariance updates
Matrix22P;// covariance matrix
Matrix22_50storedStates;// state vectors stored for the last 50 time steps
uint32_tstatetimeStamp[50];// time stamp for each state vector stored
Vector3fcorrectedDelAng;// delta angles about the xyz body axes corrected for errors (rad)
Vector3fcorrectedDelVel;// delta velocities along the XYZ body axes corrected for errors (m/s)
Vector3fsummedDelAng;// corrected & summed delta angles about the xyz body axes (rad)
Vector3fsummedDelVel;// corrected & summed delta velocities along the XYZ body axes (m/s)
Vector3fprevDelAng;// previous delta angle use for INS coning error compensation
Matrix3fprevTnb;// previous nav to body transformation used for INS earth rotation compensation
ftypeaccNavMag;// magnitude of navigation accel - used to adjust GPS obs variance (m/s^2)
Vector3fearthRateNED;// earths angular rate vector in NED (rad/s)
Vector3fdVelIMU;// delta velocity vector in XYZ body axes measured by the IMU (m/s)
Vector3fdAngIMU;// delta angle vector in XYZ body axes measured by the IMU (rad)
ftypedtIMU;// time lapsed since the last IMU measurement (sec)
ftypedt;// time lapsed since the last covariance prediction (sec)
ftypehgtRate;// state for rate of change of height filter
boolonGround;// boolean true when the flight vehicle is on the ground (not flying)
constbooluseAirspeed;// boolean true if airspeed data is being used
constbooluseCompass;// boolean true if magnetometer data is being used
Vector6innovVelPos;// innovation output for a group of measurements
Vector6varInnovVelPos;// innovation variance output for a group of measurements
boolfuseVelData;// this boolean causes the velNED measurements to be fused
boolfusePosData;// this boolean causes the posNE measurements to be fused
boolfuseHgtData;// this boolean causes the hgtMea measurements to be fused
Vector3fvelNED;// North, East, Down velocity measurements (m/s)
Vector2posNE;// North, East position measurements (m)
ftypehgtMea;// height measurement relative to reference point (m)
Vector22statesAtVelTime;// States at the effective time of velNED measurements
Vector22statesAtPosTime;// States at the effective time of posNE measurements
Vector22statesAtHgtTime;// States at the effective time of hgtMea measurement
Vector3finnovMag;// innovation output from fusion of X,Y,Z compass measurements
Vector3fvarInnovMag;// innovation variance output from fusion of X,Y,Z compass measurements
boolfuseMagData;// boolean true when magnetometer data is to be fused
Vector3fmagData;// magnetometer flux readings in X,Y,Z body axes
Vector22statesAtMagMeasTime;// filter states at the effective time of compass measurements
ftypeinnovVtas;// innovation output from fusion of airspeed measurements
ftypevarInnovVtas;// innovation variance output from fusion of airspeed measurements
boolfuseVtasData;// boolean true when airspeed data is to be fused
floatVtasMeas;// true airspeed measurement (m/s)
Vector22statesAtVtasMeasTime;// filter states at the effective measurement time
Vector3fmagBias;// magnetometer bias vector in XYZ body axes
constftypecovTimeStepMax;// maximum time allowed between covariance predictions
constftypecovDelAngMax;// maximum delta angle between covariance predictions
boolcovPredStep;// boolean set to true when a covariance prediction step has been performed
boolmagFusePerformed;// boolean set to true when magnetometer fusion has been perfomred in that time step
boolmagFuseRequired;// boolean set to true when magnetometer fusion will be perfomred in the next time step
boolposVelFuseStep;// boolean set to true when position and velocity fusion is being performed
booltasFuseStep;// boolean set to true when airspeed fusion is being performed
uint32_tTASmsecPrev;// time stamp of last TAS fusion step
constuint32_tTASmsecMax;// maximum allowed interval between TAS fusion steps
uint32_tMAGmsecPrev;// time stamp of last compass fusion step
uint32_tHGTmsecPrev;// time stamp of last height measurement fusion step
constboolfuseMeNow;// boolean to force fusion whenever data arrives
boolstaticMode;// boolean to force position and velocity measurements to zero for pre-arm or bench testing
uint32_tlastMagUpdate;// last time compass was updated
uint8_timuStepsVelFuse;// Number of IMU time steps from the last velocity fusion
Vector3faccelSumVelFuse;// sum of gravity corrected acceleration since last velocity fusion
Vector3fvelDotNED;// rate of change of velocity in NED frame
