AP_NavEKF: allow states to be accessed using names as well as a Vector22

libraries/AP_NavEKF/AP_NavEKF.cpp

@@ -199,10 +199,13 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
 
     AP_GROUPEND
 };
+
+
 // constructor
 NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) :
     _ahrs(ahrs),
     _baro(baro),
+    state(*reinterpret_cast<struct state_elements *>(&states)),
     useCompass(true),           // activates fusion of airspeed data
     covTimeStepMax(0.07f),      // maximum time (sec) between covariance prediction updates
     covDelAngMax(0.05f),        // maximum delta angle between covariance prediction updates
@@ -247,11 +250,10 @@ bool NavEKF::healthy(void) const
     if (!statesInitialised) {
         return false;
     }
-    Quaternion q(states[0],states[1],states[2],states[3]);
-    if (q.is_nan()) {
+    if (state.quat.is_nan()) {
         return false;
     }
-    if (isnan(states[4]) || isnan(states[5]) || isnan(states[6])) {
+    if (state.velocity.is_nan()) {
         return false;
     }
     // If measurements have failed innovation consistency checks for long enough to time-out
@@ -356,15 +358,15 @@ void NavEKF::InitialiseFilterDynamic(void)
     }
 
     // write to state vector
-    for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions
-    for (uint8_t j=10; j<=15; j++) states[j] = 0.0; // dAngBias, dVelBias, windVel
+    state.quat = initQuat;
+    state.gyro_bias.zero();
+    state.accel_zbias = 0;
+    state.wind_vel.zero();
     ResetVelocity();
     ResetPosition();
     ResetHeight();
-    states[16] = initMagNED.x; // Magnetic Field North
-    states[17] = initMagNED.y; // Magnetic Field East
-    states[18] = initMagNED.z; // Magnetic Field Down
-    for (uint8_t j=19; j<=21; j++) states[j] = magBias[j-19]; // Magnetic Field Bias XYZ
+    state.earth_magfield = initMagNED;
+    state.body_magfield  = magBias;
 
     statesInitialised = true;
 
@@ -447,10 +449,12 @@ void NavEKF::InitialiseFilterBootstrap(void)
     OnGroundCheck();
 
     // write to state vector
-    for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions
-    for (uint8_t j=10; j<=15; j++) states[j] = 0.0f; // dAngBias, dVelBias, windVel
-    for (uint8_t j=16; j<=18; j++) states[j] = initMagVecNED[j-16]; // Magnetic Field NED
-    for (uint8_t j=19; j<=21; j++) states[j] = initMagBias[j-19]; // Magnetic Field Bias XYZ
+    state.quat = initQuat;
+    state.gyro_bias.zero();
+    state.accel_zbias = 0;
+    state.wind_vel.zero();
+    state.earth_magfield = initMagVecNED;
+    state.body_magfield = initMagBias;
 
     statesInitialised = true;
 
@@ -661,12 +665,9 @@ void NavEKF::UpdateStrapdownEquationsNED()
     const Vector3f gravityNED(0, 0, GRAVITY_MSS);
 
     // Remove sensor bias errors
-    correctedDelAng.x = dAngIMU.x - states[10];
-    correctedDelAng.y = dAngIMU.y - states[11];
-    correctedDelAng.z = dAngIMU.z - states[12];
-    correctedDelVel.x = dVelIMU.x;
-    correctedDelVel.y = dVelIMU.y;
-    correctedDelVel.z = dVelIMU.z - states[13];
+    correctedDelAng = dAngIMU - state.gyro_bias;
+    correctedDelVel = dVelIMU;
+    correctedDelVel.z -= state.accel_zbias;
 
     // Save current measurements
     prevDelAng = correctedDelAng;
@@ -732,20 +733,13 @@ void NavEKF::UpdateStrapdownEquationsNED()
     accNavMag = velDotNED.length();
 
     // If calculating position save previous velocity
-    Vector3f lastVelocity;
-    lastVelocity.x = states[4];
-    lastVelocity.y = states[5];
-    lastVelocity.z = states[6];
+    Vector3f lastVelocity = state.velocity;
 
     // Sum delta velocities to get velocity
-    states[4] = states[4] + delVelNav.x;
-    states[5] = states[5] + delVelNav.y;
-    states[6] = states[6] + delVelNav.z;
+    state.velocity += delVelNav;
 
     // If calculating postions, do a trapezoidal integration for position
-    states[7] = states[7] + 0.5f*(states[4] + lastVelocity[0])*dtIMU;
-    states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU;
-    states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU;
+    state.position += (state.velocity + lastVelocity) * (dtIMU*0.5f);
 
     // Limit states to protect against divergence
     ConstrainStates();
@@ -1629,11 +1623,7 @@ void NavEKF::FuseVelPosNED()
                 {
                     states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex];
                 }
-                Quaternion q(states[0], states[1], states[2], states[3]);
-                q.normalize();
-                for (uint8_t i = 0; i<=3; i++) {
-                    states[i] = q[i];
-                }
+                state.quat.normalize();
 
                 // Update the covariance - take advantage of direct observation of a
                 // single state at index = stateIndex to reduce computations

libraries/AP_NavEKF/AP_NavEKF.h

@@ -259,6 +259,22 @@ private:
     bool static_mode_demanded(void) const;
 
 private:
+
+    // the states are available in two forms, either as a Vector22, or
+    // broken down as individual elements. Both are equivalent (same
+    // memory)
+    Vector22 states;
+    struct state_elements {
+        Quaternion quat;          // 0..3
+        Vector3f velocity;        // 4..6
+        Vector3f position;        // 7..9
+        Vector3f gyro_bias;       // 10..12
+        float    accel_zbias;     // 13
+        Vector2f wind_vel;        // 14..15
+        Vector3f earth_magfield;  // 16..18
+        Vector3f body_magfield;   // 19..21
+    } &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
@@ -309,7 +325,7 @@ private:
     bool velTimeout;                // boolean true if velocity measurements have failed innovation consistency check and timed out
     bool posTimeout;                // boolean true if position measurements have failed innovation consistency check and timed out
     bool hgtTimeout;                // boolean true if height measurements have failed innovation consistency check and timed out
-    Vector22 states;                // 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
+
     Vector22 Kfusion;               // Kalman gain vector
     Matrix22 KH;                    // intermediate result used for covariance updates
     Matrix22 KHP;                   // intermediate result used for covariance updates