AP_NavEKF: Updated comments in header file

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;
 
 };