AP_NavEKF: used state structure in more places

makes the code a bit easier to read

Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>

libraries/AP_NavEKF/AP_NavEKF.cpp

@@ -1565,10 +1565,8 @@ void NavEKF::FuseVelPosNED()
         // because there may be no stored states due to lack of real measurements.
         // in static mode, only position and height fusion is used
         if (staticMode) {
-            for (uint8_t i=0; i<=30; i++) {
+            statesAtPosTime = state;
-                statesAtPosTime[i] = states[i];
+            statesAtHgtTime = state;
-                statesAtHgtTime[i] = states[i];
-            }
         }
 
         // set the GPS data timeout depending on whether airspeed data is present
@@ -1606,8 +1604,8 @@ void NavEKF::FuseVelPosNED()
         bool badIMUdata = false;
             if (_fusionModeGPS == 0 && fuseVelData && fuseHgtData) {
             // calculate innovations for height and vertical GPS vel measurements
-            float hgtErr  = statesAtVelTime[9] - observation[5];
+            float hgtErr  = statesAtVelTime.position.z - observation[5];
-            float velDErr = statesAtVelTime[6] - observation[2];
+            float velDErr = statesAtVelTime.velocity.z - observation[2];
             // check if they are the same sign and both more than 3-sigma out of bounds
             if ((hgtErr*velDErr > 0.0f) && (sq(hgtErr) > 9.0f * (P[9][9] + R_OBS[5])) && (sq(velDErr) > 9.0f * (P[6][6] + R_OBS[2]))) {
                 badIMUdata = true;
@@ -1622,8 +1620,8 @@ void NavEKF::FuseVelPosNED()
         if (fusePosData)
         {
             // test horizontal position measurements
-            posInnov[0] = statesAtPosTime[7] - observation[3];
+            posInnov[0] = statesAtPosTime.position.x - observation[3];
-            posInnov[1] = statesAtPosTime[8] - observation[4];
+            posInnov[1] = statesAtPosTime.position.y - observation[4];
             varInnovVelPos[3] = P[7][7] + R_OBS[3];
             varInnovVelPos[4] = P[8][8] + R_OBS[4];
             // apply an innovation consistency threshold test, but don't fail if bad IMU data
@@ -1675,9 +1673,9 @@ void NavEKF::FuseVelPosNED()
                 // velocity states start at index 4
                 stateIndex   = i + 4;
                 // calculate innovations using blended and single IMU predicted states
-                velInnov[i]  = statesAtVelTime[stateIndex] - observation[i]; // blended
+                velInnov[i]  = statesAtVelTime.velocity[i] - observation[i]; // blended
-                velInnov1[i] = statesAtVelTime[23 + i] - observation[i]; // IMU1
+                velInnov1[i] = statesAtVelTime.vel1[i] - observation[i]; // IMU1
-                velInnov2[i] = statesAtVelTime[27 + i] - observation[i]; // IMU2
+                velInnov2[i] = statesAtVelTime.vel2[i] - observation[i]; // IMU2
                 // calculate innovation variance
                 varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i];
                 // calculate error weightings for singloe IMU velocity states using
@@ -1731,7 +1729,7 @@ void NavEKF::FuseVelPosNED()
             if (_fusionModeGPS == 0) hgtRetryTime = _hgtRetryTimeMode0;
             else hgtRetryTime = _hgtRetryTimeMode12;
             // calculate height innovations
-            hgtInnov = statesAtHgtTime[9] - observation[5];
+            hgtInnov = statesAtHgtTime.position.z - observation[5];
             varInnovVelPos[5] = P[9][9] + R_OBS[5];
             // calculate the innovation consistency test ratio
             hgtTestRatio = sq(hgtInnov) / (sq(_hgtInnovGate) * varInnovVelPos[5]);
@@ -1796,15 +1794,15 @@ void NavEKF::FuseVelPosNED()
                 // calculate the measurement innovation, using states from a different time coordinate if fusing height data
                 if (obsIndex <= 2)
                 {
-                    innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex];
+                    innovVelPos[obsIndex] = statesAtVelTime.velocity[obsIndex] - observation[obsIndex];
                 }
                 else if (obsIndex == 3 || obsIndex == 4)
                 {
-                    innovVelPos[obsIndex] = statesAtPosTime[stateIndex] - observation[obsIndex];
+                    innovVelPos[obsIndex] = statesAtPosTime.position[obsIndex-3] - observation[obsIndex];
                 }
                 else
                 {
-                    innovVelPos[obsIndex] = statesAtHgtTime[stateIndex] - observation[obsIndex];
+                    innovVelPos[obsIndex] = statesAtHgtTime.position[obsIndex-3] - observation[obsIndex];
                 }
                 // calculate the Kalman gain and calculate innovation variances
                 varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex];
@@ -1824,8 +1822,8 @@ void NavEKF::FuseVelPosNED()
                 // Correct states that have been predicted using single (not blended) IMU data
                 if (obsIndex == 5){
                     // Calculate height measurement innovations using single IMU states
-                    float hgtInnov1 = statesAtHgtTime[26] - observation[obsIndex];
+                    float hgtInnov1 = statesAtHgtTime.posD1 - observation[obsIndex];
-                    float hgtInnov2 = statesAtHgtTime[30] - observation[obsIndex];
+                    float hgtInnov2 = statesAtHgtTime.posD2 - observation[obsIndex];
                     // Correct single IMU prediction states using height measurement, limiting rate of change of bias to 0.02 m/s3
                     float correctionLimit = 0.02f * dtIMU *dtVelPos;
                     states[13] = states[13] - constrain_float(Kfusion[13] * hgtInnov1, -correctionLimit, correctionLimit); // IMU1 Z accel bias
@@ -1943,16 +1941,16 @@ void NavEKF::FuseMagnetometer()
         if (fuseMagData)
         {
             // copy required states to local variable names
-            q0       = statesAtMagMeasTime[0];
+            q0       = statesAtMagMeasTime.quat[0];
-            q1       = statesAtMagMeasTime[1];
+            q1       = statesAtMagMeasTime.quat[1];
-            q2       = statesAtMagMeasTime[2];
+            q2       = statesAtMagMeasTime.quat[2];
-            q3       = statesAtMagMeasTime[3];
+            q3       = statesAtMagMeasTime.quat[3];
-            magN     = statesAtMagMeasTime[16];
+            magN     = statesAtMagMeasTime.earth_magfield[0];
-            magE     = statesAtMagMeasTime[17];
+            magE     = statesAtMagMeasTime.earth_magfield[1];
-            magD     = statesAtMagMeasTime[18];
+            magD     = statesAtMagMeasTime.earth_magfield[2];
-            magXbias = statesAtMagMeasTime[19];
+            magXbias = statesAtMagMeasTime.body_magfield[0];
-            magYbias = statesAtMagMeasTime[20];
+            magYbias = statesAtMagMeasTime.body_magfield[1];
-            magZbias = statesAtMagMeasTime[21];
+            magZbias = statesAtMagMeasTime.body_magfield[2];
 
             // rotate predicted earth components into body axes and calculate
             // predicted measurements
@@ -2240,11 +2238,11 @@ void NavEKF::FuseAirspeed()
     float VtasPred;
 
     // copy required states to local variable names
-    vn = statesAtVtasMeasTime[4];
+    vn = statesAtVtasMeasTime.velocity.x;
-    ve = statesAtVtasMeasTime[5];
+    ve = statesAtVtasMeasTime.velocity.y;
-    vd = statesAtVtasMeasTime[6];
+    vd = statesAtVtasMeasTime.velocity.z;
-    vwn = statesAtVtasMeasTime[14];
+    vwn = statesAtVtasMeasTime.wind_vel.x;
-    vwe = statesAtVtasMeasTime[15];
+    vwe = statesAtVtasMeasTime.wind_vel.y;
 
     // calculate the predicted airspeed
     VtasPred = pythagorous3((ve - vwe) , (vn - vwn) , vd);
@@ -2556,9 +2554,7 @@ void NavEKF::StoreStates()
         if (storeIndex > 49) {
             storeIndex = 0;
         }
-        for (uint8_t i=0; i<=30; i++) {
+        storedStates[storeIndex] = state;
-            storedStates[i][storeIndex] = states[i];
-        }
         statetimeStamp[storeIndex] = lastStateStoreTime_ms;
         storeIndex = storeIndex + 1;
     }
@@ -2568,17 +2564,17 @@ void NavEKF::StoreStates()
 void NavEKF::StoreStatesReset()
 {
     // clear stored state history
-    memset(&storedStates[0][0], 0, sizeof(storedStates));
+    memset(&storedStates[0], 0, sizeof(storedStates));
     memset(&statetimeStamp[0], 0, sizeof(statetimeStamp));
     // store current state vector in first column
     storeIndex = 0;
-    for (uint8_t i=0; i<=30; i++) storedStates[i][storeIndex] = states[i];
+    storedStates[storeIndex] = state;
     statetimeStamp[storeIndex] = hal.scheduler->millis();
     storeIndex = storeIndex + 1;
 }
 
 // recall state vector stored at closest time to the one specified by msec
-void NavEKF::RecallStates(Vector31 &statesForFusion, uint32_t msec)
+void NavEKF::RecallStates(state_elements &statesForFusion, uint32_t msec)
 {
     uint32_t timeDelta;
     uint32_t bestTimeDelta = 200;
@@ -2594,15 +2590,11 @@ void NavEKF::RecallStates(Vector31 &statesForFusion, uint32_t msec)
     }
     if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error
     {
-        for (uint8_t i=0; i<=30; i++) {
+        statesForFusion = storedStates[bestStoreIndex];
-            statesForFusion[i] = storedStates[i][bestStoreIndex];
-        }
     }
     else // otherwise output current state
     {
-        for (uint8_t i=0; i<=30; i++) {
+        statesForFusion = state;
-            statesForFusion[i] = states[i];
-        }
     }
 }
 
@@ -3221,7 +3213,7 @@ void NavEKF::ZeroVariables()
     memset(&P[0][0], 0, sizeof(P));
     memset(&nextP[0][0], 0, sizeof(nextP));
     memset(&processNoise[0], 0, sizeof(processNoise));
-    memset(&storedStates[0][0], 0, sizeof(storedStates));
+    memset(&storedStates[0], 0, sizeof(storedStates));
     memset(&statetimeStamp[0], 0, sizeof(statetimeStamp));
     memset(&posNE[0], 0, sizeof(posNE));
 }

libraries/AP_NavEKF/AP_NavEKF.h

@@ -141,6 +141,26 @@ private:
     const AP_AHRS *_ahrs;
     AP_Baro &_baro;
 
+    // the states are available in two forms, either as a Vector27, or
+    // broken down as individual elements. Both are equivalent (same
+    // memory)
+    Vector31 states;
+    struct state_elements {
+        Quaternion  quat;           // 0..3
+        Vector3f    velocity;       // 4..6
+        Vector3f    position;       // 7..9
+        Vector3f    gyro_bias;      // 10..12
+        float       accel_zbias1;   // 13
+        Vector2f    wind_vel;       // 14..15
+        Vector3f    earth_magfield; // 16..18
+        Vector3f    body_magfield;  // 19..21
+        float       accel_zbias2;   // 22
+        Vector3f    vel1;           // 23 .. 25
+        float       posD1;          // 26
+        Vector3f    vel2;           // 27 .. 29
+        float       posD2;          // 30
+    } &state;
+
     // update the quaternion, velocity and position states using IMU measurements
     void UpdateStrapdownEquationsNED();
 
@@ -184,7 +204,7 @@ private:
     void StoreStatesReset(void);
 
     // recall state vector stored at closest time to the one specified by msec
-    void RecallStates(Vector31 &statesForFusion, uint32_t msec);
+    void RecallStates(state_elements &statesForFusion, uint32_t msec);
 
     // calculate nav to body quaternions from body to nav rotation matrix
     void quat2Tbn(Matrix3f &Tbn, const Quaternion &quat) const;
@@ -256,28 +276,6 @@ private:
     // this allows large GPS position jumps to be accomodated gradually
     void decayGpsOffset(void);
 
-private:
-
-    // the states are available in two forms, either as a Vector27, or
-    // broken down as individual elements. Both are equivalent (same
-    // memory)
-    Vector31 states;
-    struct state_elements {
-        Quaternion  quat;           // 0..3
-        Vector3f    velocity;       // 4..6
-        Vector3f    position;       // 7..9
-        Vector3f    gyro_bias;      // 10..12
-        float       accel_zbias1;   // 13
-        Vector2f    wind_vel;       // 14..15
-        Vector3f    earth_magfield; // 16..18
-        Vector3f    body_magfield;  // 19..21
-        float       accel_zbias2;   // 22
-        Vector3f    vel1;           // 23 .. 25
-        float       posD1;          // 26
-        Vector3f    vel2;           // 27 .. 29
-        float       posD2;          // 30
-    } &state;
-
     // EKF Mavlink Tuneable Parameters
     AP_Float _gpsHorizVelNoise;     // GPS horizontal velocity measurement noise : m/s
     AP_Float _gpsVertVelNoise;      // GPS vertical velocity measurement noise : m/s
@@ -337,7 +335,7 @@ private:
     Matrix22 KH;                    // intermediate result used for covariance updates
     Matrix22 KHP;                   // intermediate result used for covariance updates
     Matrix22 P;                     // covariance matrix
-    Matrix31_50 storedStates;       // state vectors stored for the last 50 time steps
+    VectorN<state_elements,50> storedStates;       // 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 correctedDelVel12;     // delta velocities along the XYZ body axes for weighted average of IMU1 and IMU2 corrected for errors (m/s)
@@ -366,19 +364,19 @@ private:
     Vector3f velNED;                // North, East, Down velocity measurements (m/s)
     Vector2 posNE;                  // North, East position measurements (m)
     ftype hgtMea;                   //  height measurement relative to reference point  (m)
-    Vector31 statesAtVelTime;       // States at the effective time of velNED measurements
+    state_elements statesAtVelTime; // States at the effective time of velNED measurements
-    Vector31 statesAtPosTime;       // States at the effective time of posNE measurements
+    state_elements statesAtPosTime; // States at the effective time of posNE measurements
-    Vector31 statesAtHgtTime;       // States at the effective time of hgtMea measurement
+    state_elements statesAtHgtTime; // States at the effective time of hgtMea measurement
     Vector3f innovMag;              // innovation output from fusion of X,Y,Z compass measurements
     Vector3f varInnovMag;           // 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 readings in X,Y,Z body axes
-    Vector31 statesAtMagMeasTime;   // filter states at the effective time of compass measurements
+    state_elements statesAtMagMeasTime;   // filter states at the effective time of compass measurements
     ftype innovVtas;                // innovation output from fusion of airspeed measurements
     ftype 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)
-    Vector31 statesAtVtasMeasTime;  // filter states at the effective measurement time
+    state_elements statesAtVtasMeasTime;  // filter states at the effective measurement time
     Vector3f magBias;               // magnetometer bias vector in XYZ body axes
     const ftype covTimeStepMax;     // maximum time allowed between covariance predictions
     const ftype covDelAngMax;       // maximum delta angle between covariance predictions