diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 79c5e32543..eaf0770cbe 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -136,55 +136,57 @@ private: float KH[24][24]; // intermediate result used for covariance updates float KHP[24][24]; // intermediate result used for covariance updates float P[24][24]; // covariance matrix - float states[24]; // state matrix + float states[24]; // 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 float storedStates[24][50]; // state vectors stored for the last 50 time steps uint32_t statetimeStamp[50]; // time stamp for each state vector stored Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) - Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) - Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) - float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) + Vector3f summedDelAng; // corrected & summed delta angles about the xyz body axes (rad) + Vector3f summedDelVel; // corrected & summed delta velocities along the XYZ body axes (m/s) + Vector3f prevDelAng; // previous delta angle use for INS coning error compensation + Matrix3f prevTnb; // previous nav to body transformation used for INS earth rotation compensation + float accNavMag; // magnitude of navigation accel - used to adjust GPS obs variance (m/s^2) Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) Vector3f dVelIMU; // delta velocity vector in XYZ body axes measured by the IMU (m/s) Vector3f dAngIMU; // delta angle vector in XYZ body axes measured by the IMU (rad) - float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) - float dt; // time lapsed since last covariance prediction + float dtIMU; // time lapsed since the last IMU measurement (sec) + float dt; // time lapsed since the last covariance prediction (sec) bool onGround; // boolean true when the flight vehicle is on the ground (not flying) const bool useAirspeed; // boolean true if airspeed data is being used const bool useCompass; // boolean true if magnetometer data is being used const uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity - float innovVelPos[6]; // innovation output - float varInnovVelPos[6]; // innovation variance output - bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused - bool fusePosData; // this boolean causes the posNE and velNED obs to be fused - bool fuseHgtData; // this boolean causes the hgtMea obs to be fused - float velNED[3]; // North, East, Down velocity obs (m/s) - float posNE[2]; // North, East position obs (m) - float hgtMea; // measured height (m) - float posNED[3]; // North, East Down position (m) - float statesAtVelTime[24]; // States at the effective measurement time for posNE and velNED measurements - float statesAtPosTime[24]; // States at the effective measurement time for posNE and velNED measurements - float statesAtHgtTime[24]; // States at the effective measurement time for the hgtMea measurement - float innovMag[3]; // innovation output - float varInnovMag[3]; // innovation variance output + float innovVelPos[6]; // innovation output for a group of measurements + float varInnovVelPos[6]; // innovation variance output for a group of measurements + 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 + float velNED[3]; // North, East, Down velocity measurements (m/s) + float posNE[2]; // North, East position measurements (m) + float hgtMea; // height measurement relative to reference point (m) + float posNED[3]; // North, East Down position relative to reference point (m) + float statesAtVelTime[24]; // States at the effective time of velNED measurements + float statesAtPosTime[24]; // States at the effective time of posNE measurements + float statesAtHgtTime[24]; // States at the effective time of hgtMea measurement + float innovMag[3]; // innovation output from fusion of X,Y,Z compass measurements + float varInnovMag[3]; // innovation variance output from fusion of X,Y,Z compass measurements bool fuseMagData; // boolean true when magnetometer data is to be fused - Vector3f magData; // magnetometer flux radings in X,Y,Z body axes - float statesAtMagMeasTime[24]; // filter satates at the effective measurement time - float innovVtas; // innovation output - float varInnovVtas; // innovation variance output + Vector3f magData; // magnetometer flux readings in X,Y,Z body axes + float statesAtMagMeasTime[24]; // filter states at the effective time of compass measurements + float innovVtas; // innovation output from fusion of airspeed measurements + float varInnovVtas; // innovation variance output from fusion of airspeed measurements bool fuseVtasData; // boolean true when airspeed data is to be fused float VtasMeas; // true airspeed measurement (m/s) float statesAtVtasMeasTime[24]; // filter states at the effective measurement time float latRef; // WGS-84 latitude of reference point (rad) float lonRef; // WGS-84 longitude of reference point (rad) float hgtRef; // WGS-84 height of reference point (m) - Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes + Vector3f magBias; // magnetometer bias vector in XYZ body axes float eulerEst[3]; // Euler angles calculated from filter states float eulerDif[3]; // difference between Euler angle estimated by EKF and the AHRS solution const float covTimeStepMax; // maximum time allowed between covariance predictions const float covDelAngMax; // maximum delta angle between covariance predictions bool covPredStep; // boolean set to true when a covariance prediction step has been performed - bool magFuseStep; // boolean set to true when magnetometer fusion steps are being performed + bool magFuseStep; // boolean set to true when magnetometer fusion is being performed bool posVelFuseStep; // boolean set to true when position and velocity fusion is being performed bool tasFuseStep; // boolean set to true when airspeed fusion is being performed uint32_t TASmsecPrev; // time stamp of last TAS fusion step @@ -194,7 +196,7 @@ private: uint32_t HGTmsecPrev; // time stamp of last height measurement fusion step const uint32_t HGTmsecTgt; // target interval between height measurement fusion steps - // Estimated time delays (msec) + // Estimated time delays (msec) for different measurements relative to IMU const uint32_t msecVelDelay; const uint32_t msecPosDelay; const uint32_t msecHgtDelay; @@ -226,13 +228,14 @@ private: // AHRS input data variables float ahrsEul[3]; + // Time stamp when vel, pos or height measurements last failed checks uint32_t velFailTime; uint32_t posFailTime; uint32_t hgtFailTime; - Vector3f prevDelAng; - Matrix3f prevTnb; - + // states held by magnetomter fusion across time steps + // magnetometer X,Y,Z measurements are fused across three time steps + // to struct { float q0; float q1; @@ -251,9 +254,13 @@ private: float SH_MAG[9]; } mag_state; + // State vector storage index uint8_t storeIndex; + // high precision time stamp for previous IMU data processing uint32_t lastIMUusec; + + // time of alst GPS fix used to determine if new data has arrived uint32_t lastFixTime; };