constuint16_tgpsRetryTimeUseTAS;// GPS retry time with airspeed measurements (msec)
constuint16_tgpsRetryTimeNoTAS;// GPS retry time without airspeed measurements (msec)
constuint16_tgpsFailTimeWithFlow;// If we have no GPs for longer than this and we have optical flow, then we will switch across to using optical flow (msec)
constuint16_thgtRetryTimeMode0;// Height retry time with vertical velocity measurement (msec)
constuint16_thgtRetryTimeMode12;// Height retry time without vertical velocity measurement (msec)
constuint16_ttasRetryTime;// True airspeed timeout and retry interval (msec)
constuint32_tmagFailTimeLimit_ms;// number of msec before a magnetometer failing innovation consistency checks is declared failed (msec)
constfloatmagVarRateScale;// scale factor applied to magnetometer variance due to angular rate
constfloatgyroBiasNoiseScaler;// scale factor applied to gyro bias state process noise when on ground
constfloataccelBiasNoiseScaler;// scale factor applied to accel bias state process noise when on ground
constuint16_tmsecGpsAvg;// average number of msec between GPS measurements
constuint16_tmsecHgtAvg;// average number of msec between height measurements
constuint16_tmsecMagAvg;// average number of msec between magnetometer measurements
constuint16_tmsecBetaAvg;// average number of msec between synthetic sideslip measurements
constuint16_tmsecBetaMax;// maximum number of msec between synthetic sideslip measurements
constuint16_tmsecFlowAvg;// average number of msec between optical flow measurements
constfloatdtVelPos;// number of seconds between position and velocity corrections. This should be a multiple of the imu update interval.
constfloatcovTimeStepMax;// maximum time (sec) between covariance prediction updates
constfloatcovDelAngMax;// maximum delta angle between covariance prediction updates
constuint32_tTASmsecMax;// maximum allowed interval between airspeed measurement updates
constfloatDCM33FlowMin;// If Tbn(3,3) is less than this number, optical flow measurements will not be fused as tilt is too high.
constfloatfScaleFactorPnoise;// Process noise added to focal length scale factor state variance at each time step
constuint8_tflowTimeDeltaAvg_ms;// average interval between optical flow measurements (msec)
constuint32_tflowIntervalMax_ms;// maximum allowable time between flow fusion events
// ground effect tuning parameters
constuint16_tgndEffectTimeout_ms;// time in msec that ground effect mode is active after being activated
constfloatgndEffectBaroScaler;// scaler applied to the barometer observation variance when ground effect mode is active
// Variables
boolstatesInitialised;// boolean true when filter states have been initialised
boolvelHealth;// boolean true if velocity measurements have passed innovation consistency check
boolposHealth;// boolean true if position measurements have passed innovation consistency check
boolhgtHealth;// boolean true if height measurements have passed innovation consistency check
boolmagHealth;// boolean true if magnetometer has passed innovation consistency check
booltasHealth;// boolean true if true airspeed has passed 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
boolmagTimeout;// boolean true if magnetometer measurements have failed for too long and have timed out
booltasTimeout;// boolean true if true airspeed measurements have failed for too long and have timed out
boolbadMag;// boolean true if the magnetometer is declared to be producing bad data
boolbadIMUdata;// boolean true if the bad IMU data is detected
floatgpsNoiseScaler;// Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts
Vector31Kfusion;// Kalman gain vector
Matrix22KH;// intermediate result used for covariance updates
Matrix22KHP;// intermediate result used for covariance updates
Matrix22P;// covariance matrix
VectorN<state_elements,50>storedStates;// state vectors stored for the last 50 time steps
Vector_u32_50statetimeStamp;// time stamp for each state vector stored
Vector3fcorrectedDelAng;// delta angles about the xyz body axes corrected for errors (rad)
QuaternioncorrectedDelAngQuat;// quaternion representation of correctedDelAng
Vector3fcorrectedDelVel12;// delta velocities along the XYZ body axes for weighted average of IMU1 and IMU2 corrected for errors (m/s)
Vector3fcorrectedDelVel1;// delta velocities along the XYZ body axes for IMU1 corrected for errors (m/s)
Vector3fcorrectedDelVel2;// delta velocities along the XYZ body axes for IMU2 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)
Vector3flastGyroBias;// previous gyro bias vector used by filter divergence check
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)
ftypeaccNavMagHoriz;// magnitude of navigation accel in horizontal plane (m/s^2)
Vector3fearthRateNED;// earths angular rate vector in NED (rad/s)
Vector3fdVelIMU1;// delta velocity vector in XYZ body axes measured by IMU1 (m/s)
Vector3fdVelIMU2;// delta velocity vector in XYZ body axes measured by IMU2 (m/s)
Vector3fdAngIMU;// delta angle vector in XYZ body axes measured by the IMU (rad)
ftypedtIMUavg;// expected time between IMU measurements (sec)
ftypedtDelAng;// 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)
boolprevOnGround;// value of onGround from previous update
boolmanoeuvring;// boolean true when the flight vehicle is performing horizontal changes in velocity
uint32_tairborneDetectTime_ms;// last time flight movement was detected
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)
Vector2fgpsPosNE;// North, East position measurements (m)
ftypehgtMea;// height measurement relative to reference point (m)
state_elementsstatesAtVelTime;// States at the effective time of velNED measurements
state_elementsstatesAtPosTime;// States at the effective time of posNE measurements
state_elementsstatesAtHgtTime;// 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
Vector3fmagData;// magnetometer flux readings in X,Y,Z body axes
state_elementsstatesAtMagMeasTime;// 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)
state_elementsstatesAtVtasMeasTime;// filter states at the effective measurement time
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
uint32_tBETAmsecPrev;// time stamp of last synthetic sideslip fusion step
uint32_tMAGmsecPrev;// time stamp of last compass fusion step
uint32_tHGTmsecPrev;// time stamp of last height measurement fusion step
boolconstPosMode;// true when fusing a constant position to maintain attitude reference for planned operation without GPS or optical flow data
uint32_tlastMagUpdate;// last time compass was updated
Vector3fvelDotNED;// rate of change of velocity in NED frame
uint32_tsecondLastFixTime_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
Matrix22nextP;// Predicted covariance matrix before addition of process noise to diagonals
Vector22processNoise;// process noise added to diagonals of predicted covariance matrix
Vector15SF;// intermediate variables used to calculate predicted covariance matrix
Vector8SG;// intermediate variables used to calculate predicted covariance matrix
Vector11SQ;// intermediate variables used to calculate predicted covariance matrix
Vector8SPP;// 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
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
boolgpsNotAvailable;// bool true when valid GPS data is not available
boolvehicleArmed;// true when the vehicle is disarmed
boolprevVehicleArmed;// 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.
uint32_tlastYawReset_ms;// System time at which the last yaw reset occurred. Returned by getLastYawResetAngle
uint32_tmagYawResetTimer_ms;// timer in msec used to track how long good magnetometer data is failing innovation consistency checks
boolconsistentMagData;// true when the magnetometers are passing consistency checks
boolgpsAccuracyGood;// true when the GPS accuracy is considered to be good enough for safe flight.
uint32_ttimeAtDisarm_ms;// time of last disarm event in msec
floatgpsDriftNE;// amount of drift detected in the GPS position during pre-flight GPs checks
Vector10gpsIncrStateDelta;// vector of corrections to attitude, velocity and position to be applied over the period between the current and next GPS measurement
Vector10hgtIncrStateDelta;// vector of corrections to attitude, velocity and position to be applied over the period between the current and next height measurement
Vector10magIncrStateDelta;// vector of corrections to attitude, velocity and position to be applied over the period between the current and next magnetometer measurement
uint8_tgpsUpdateCount;// count of the number of minor state corrections using GPS data
uint8_tgpsUpdateCountMax;// limit on the number of minor state corrections using GPS data
floatgpsUpdateCountMaxInv;// floating point inverse of gpsFilterCountMax
uint8_thgtUpdateCount;// count of the number of minor state corrections using Baro data
uint8_thgtUpdateCountMax;// limit on the number of minor state corrections using Baro data
floathgtUpdateCountMaxInv;// floating point inverse of hgtFilterCountMax
uint8_tmagUpdateCount;// count of the number of minor state corrections using Magnetometer data
uint8_tmagUpdateCountMax;// limit on the number of minor state corrections using Magnetometer data
floatmagUpdateCountMaxInv;// floating point inverse of magFilterCountMax
// variables added for optical flow fusion
boolnewDataFlow;// true when new optical flow data has arrived
boolflowFusePerformed;// true when optical flow fusion has been performed in that time step
boolflowDataValid;// true while optical flow data is still fresh
state_elementsstatesAtFlowTime;// States at the middle of the optical flow sample period
boolfuseOptFlowData;// this boolean causes the last optical flow measurement to be fused
floatauxFlowObsInnov;// optical flow rate innovation from 1-state terrain offset estimator
floatauxFlowObsInnovVar;// innovation variance for optical flow observations from 1-state terrain offset estimator
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
state_elementsstatesAtRngTime;// States at the range finder measurement time
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
uint8_tflowUpdateCount;// count of the number of minor state corrections using optical flow data
uint8_tflowUpdateCountMax;// limit on the number of minor state corrections using optical flow data
floatflowUpdateCountMaxInv;// floating point inverse of flowUpdateCountMax
Vector10flowIncrStateDelta;// vector of corrections to attitude, velocity and position to be applied over the period between the current and next magnetometer measurement
boolnewDataRng;// true when new valid range finder data has arrived.
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
// 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
// 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
floatdtDelVel1;
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
// monitoring IMU quality
floatimuNoiseFiltState1;// peak hold noise estimate for IMU 1
floatimuNoiseFiltState2;// peak hold noise estimate for IMU 2
Vector3faccelDiffFilt;// filtered difference between IMU 1 and 2
enumImuSwitchState{
IMUSWITCH_MIXED=0,// IMU 0 & 1 are mixed
IMUSWITCH_IMU0,// only IMU 0 is used
IMUSWITCH_IMU1// only IMU 1 is used
};
ImuSwitchStatelastImuSwitchState;// last switch state (see imuSwitchState enum)
// states held by optical flow fusion across time steps
// optical flow X,Y motion compensated rate measurements are fused across two time steps
// to level computational load as this can be an expensive operation
struct{
uint8_tobsIndex;
Vector4SH_LOS;
Vector10SK_LOS;
ftypeq0;
ftypeq1;
ftypeq2;
ftypeq3;
ftypevn;
ftypeve;
ftypevd;
ftypepd;
Vector2losPred;
}flow_state;
struct{
boolbad_xmag:1;
boolbad_ymag:1;
boolbad_zmag:1;
boolbad_airspeed:1;
boolbad_sideslip:1;
}faultStatus;
// flags indicating which GPS quality checks are failing
struct{
boolbad_sAcc:1;
boolbad_hAcc:1;
boolbad_yaw:1;
boolbad_sats:1;
boolbad_VZ:1;
boolbad_horiz_drift:1;
boolbad_hdop:1;
boolbad_vert_vel:1;
boolbad_fix:1;
boolbad_horiz_vel:1;
}gpsCheckStatus;
// 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
struct{
ftypeq0;
ftypeq1;
ftypeq2;
ftypeq3;
ftypemagN;
ftypemagE;
ftypemagD;
ftypemagXbias;
ftypemagYbias;
ftypemagZbias;
uint8_tobsIndex;
Matrix3fDCM;
Vector3fMagPred;
ftypeR_MAG;
Vector9SH_MAG;
}mag_state;
// string representing last reason for prearm failure
