From 07d621c4be69934d47df58852af01c9185fa6766 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 4 Apr 2014 21:30:16 +1100 Subject: [PATCH] AP_NavEKF: used state structure in more places makes the code a bit easier to read Pair-Programmed-With: Paul Riseborough --- libraries/AP_NavEKF/AP_NavEKF.cpp | 82 ++++++++++++++----------------- libraries/AP_NavEKF/AP_NavEKF.h | 56 ++++++++++----------- 2 files changed, 64 insertions(+), 74 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 6073895ea5..69f5a058b4 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/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[i] = states[i]; - statesAtHgtTime[i] = states[i]; - } + statesAtPosTime = state; + statesAtHgtTime = state; } // 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 velDErr = statesAtVelTime[6] - observation[2]; + float hgtErr = statesAtVelTime.position.z - observation[5]; + 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[1] = statesAtPosTime[8] - observation[4]; + posInnov[0] = statesAtPosTime.position.x - observation[3]; + 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 - velInnov1[i] = statesAtVelTime[23 + i] - observation[i]; // IMU1 - velInnov2[i] = statesAtVelTime[27 + i] - observation[i]; // IMU2 + velInnov[i] = statesAtVelTime.velocity[i] - observation[i]; // blended + velInnov1[i] = statesAtVelTime.vel1[i] - observation[i]; // IMU1 + 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 hgtInnov2 = statesAtHgtTime[30] - observation[obsIndex]; + float hgtInnov1 = statesAtHgtTime.posD1 - 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]; - q1 = statesAtMagMeasTime[1]; - q2 = statesAtMagMeasTime[2]; - q3 = statesAtMagMeasTime[3]; - magN = statesAtMagMeasTime[16]; - magE = statesAtMagMeasTime[17]; - magD = statesAtMagMeasTime[18]; - magXbias = statesAtMagMeasTime[19]; - magYbias = statesAtMagMeasTime[20]; - magZbias = statesAtMagMeasTime[21]; + q0 = statesAtMagMeasTime.quat[0]; + q1 = statesAtMagMeasTime.quat[1]; + q2 = statesAtMagMeasTime.quat[2]; + q3 = statesAtMagMeasTime.quat[3]; + magN = statesAtMagMeasTime.earth_magfield[0]; + magE = statesAtMagMeasTime.earth_magfield[1]; + magD = statesAtMagMeasTime.earth_magfield[2]; + magXbias = statesAtMagMeasTime.body_magfield[0]; + magYbias = statesAtMagMeasTime.body_magfield[1]; + 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]; - ve = statesAtVtasMeasTime[5]; - vd = statesAtVtasMeasTime[6]; - vwn = statesAtVtasMeasTime[14]; - vwe = statesAtVtasMeasTime[15]; + vn = statesAtVtasMeasTime.velocity.x; + ve = statesAtVtasMeasTime.velocity.y; + vd = statesAtVtasMeasTime.velocity.z; + vwn = statesAtVtasMeasTime.wind_vel.x; + 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[i][storeIndex] = states[i]; - } + storedStates[storeIndex] = state; 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[i] = storedStates[i][bestStoreIndex]; - } + statesForFusion = storedStates[bestStoreIndex]; } else // otherwise output current state { - for (uint8_t i=0; i<=30; i++) { - statesForFusion[i] = states[i]; - } + statesForFusion = state; } } @@ -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)); } diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 499b155afc..349bf888ba 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/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 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 - Vector31 statesAtPosTime; // States at the effective time of posNE measurements - Vector31 statesAtHgtTime; // States at the effective time of hgtMea measurement + state_elements statesAtVelTime; // States at the effective time of velNED measurements + state_elements statesAtPosTime; // States at the effective time of posNE measurements + 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