diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 1ad8575d8d..f21a50a698 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -1,8 +1,11 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -// #define MATH_CHECK_INDEXES 1 - #include + +// uncomment this to force the optimisation of this code, note that +// this makes debugging harder +// #pragma GCC optimize("O3") + #include "AP_NavEKF.h" #include @@ -342,14 +345,6 @@ void NavEKF::CovariancePrediction() float dvy_b; float dvz_b; - // arrays - Vector24 processNoise; - VectorN SF; - VectorN SG; - VectorN SQ; - VectorN SPP; - Matrix24 nextP; - // calculate covariance prediction process noise windVelSigma = dt * 0.1f; dAngBiasSigma = dt * 5.0e-7f; @@ -1072,7 +1067,7 @@ void NavEKF::CovariancePrediction() // growth by setting the predicted to the previous values // This prevent an ill conditioned matrix from occurring for long periods // without GPS - if ((P[7][7] + P[8][8]) > 1E6) + if ((P[7][7] + P[8][8]) > 1e6f) { for (uint8_t i=7; i<=8; i++) { @@ -1107,7 +1102,7 @@ void NavEKF::FuseVelPosNED() uint32_t horizRetryTime = 0; bool velHealth = false; bool posHealth = false; - bool hgtHealth; + bool hgtHealth = false; bool velTimeout; bool posTimeout; bool hgtTimeout; @@ -1115,7 +1110,7 @@ void NavEKF::FuseVelPosNED() // declare variables used to check measurement errors Vector3f velInnov; - VectorN posInnov; + Vector2 posInnov; float hgtInnov = 0; // declare variables used to control access to arrays @@ -1129,7 +1124,6 @@ void NavEKF::FuseVelPosNED() Vector6 R_OBS; Vector6 observation; float SK; - float quatMag; // Perform sequential fusion of GPS measurements. This assumes that the // errors in the different velocity and position components are @@ -1253,7 +1247,7 @@ void NavEKF::FuseVelPosNED() stateIndex = 4 + obsIndex; // Calculate the measurement innovation, using states from a // different time coordinate if fusing height data - if (obsIndex >= 0 && obsIndex <= 2) + if (obsIndex <= 2) { innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex]; } @@ -1261,7 +1255,7 @@ void NavEKF::FuseVelPosNED() { innovVelPos[obsIndex] = statesAtPosTime[stateIndex] - observation[obsIndex]; } - else if (obsIndex == 5) + else { innovVelPos[obsIndex] = statesAtHgtTime[stateIndex] - observation[obsIndex]; } @@ -1617,7 +1611,6 @@ void NavEKF::FuseAirspeed() Vector24 H_TAS; Vector24 Kfusion; float VtasPred; - float quatMag; // Copy required states to local variable names vn = statesAtVtasMeasTime[4]; @@ -1732,26 +1725,18 @@ void NavEKF::FuseAirspeed() void NavEKF::zeroRows(Matrix24 &covMat, uint8_t first, uint8_t last) { uint8_t row; - uint8_t col; for (row=first; row<=last; row++) { - for (col=0; col<=23; col++) - { - covMat[row][col] = 0; - } + memset(&covMat[row][0], 0, sizeof(float)*24); } } void NavEKF::zeroCols(Matrix24 &covMat, uint8_t first, uint8_t last) { uint8_t row; - uint8_t col; - for (col=first; col<=last; col++) + for (row=0; row<=23; row++) { - for (row=0; row<=23; row++) - { - covMat[row][col] = 0; - } + memset(&covMat[row][first], 0, sizeof(float)*(1+last-first)); } } @@ -1773,7 +1758,6 @@ void NavEKF::RecallStates(Vector24 &statesForFusion, uint32_t msec) for (uint8_t i=0; i<=49; i++) { timeDelta = msec - statetimeStamp[i]; - if (timeDelta < 0) timeDelta = -timeDelta; if (timeDelta < bestTimeDelta) { bestStoreIndex = i; @@ -1867,9 +1851,9 @@ void NavEKF::calcposNE(float lat, float lon) bool NavEKF::getLLH(struct Location &loc) { - loc.lat = 1.0e7 * degrees(latRef + states[7] / RADIUS_OF_EARTH); - loc.lng = 1.0e7 * degrees(lonRef + (states[8] / RADIUS_OF_EARTH) / cosf(latRef)); - loc.alt = 1.0e2 * (hgtRef - states[9]); + loc.lat = 1.0e7f * degrees(latRef + states[7] / RADIUS_OF_EARTH); + loc.lng = 1.0e7f * degrees(lonRef + (states[8] / RADIUS_OF_EARTH) / cosf(latRef)); + loc.alt = 1.0e2f * (hgtRef - states[9]); return true; } @@ -1957,35 +1941,31 @@ void NavEKF::readHgtData() void NavEKF::readMagData() { // scale compass data to improve numerical conditioning - magData = _ahrs.get_compass()->get_field() * 0.001f; - magBias = _ahrs.get_compass()->get_offsets() * 0.001f; - if ((magData.x != magDataPrev.x) && (magData.y != magDataPrev.y) && (magData.z != magDataPrev.z)) - { - magDataPrev = magData; + if (_ahrs.get_compass()->last_update != lastMagUpdate) { + lastMagUpdate = _ahrs.get_compass()->last_update; + + magData = _ahrs.get_compass()->get_field() * 0.001f; + magBias = _ahrs.get_compass()->get_offsets() * 0.001f; + // Recall states from compass measurement time RecallStates(statesAtMagMeasTime, (IMUmsec - msecMagDelay)); newDataMag = true; - } - else - { + } else { newDataMag = false; } } void NavEKF::readAirSpdData() { - if (!_ahrs.airspeed_estimate_true(&VtasMeas)) - { - if (VtasMeas != VtasMeasPrev) - { - newDataTas = true; - VtasMeasPrev = VtasMeas; - RecallStates(statesAtVtasMeasTime, (IMUmsec - msecTasDelay)); - } - else - { - newDataTas = false; - } + const AP_Airspeed *aspeed = _ahrs.get_airspeed(); + if (aspeed && + aspeed->last_update_ms() != lastAirspeedUpdate && + _ahrs.airspeed_estimate_true(&VtasMeas)) { + lastAirspeedUpdate = aspeed->last_update_ms(); + newDataTas = true; + RecallStates(statesAtVtasMeasTime, (IMUmsec - msecTasDelay)); + } else { + newDataTas = false; } } diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 122cb4f5c1..5583b1df36 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -28,18 +28,40 @@ #include #include #include + +// #define MATH_CHECK_INDEXES 1 + #include + class NavEKF { public: - +#if MATH_CHECK_INDEXES typedef VectorN Vector2; + typedef VectorN Vector3; typedef VectorN Vector6; typedef VectorN Vector8; + typedef VectorN Vector11; + typedef VectorN Vector13; + typedef VectorN Vector21; typedef VectorN Vector24; + typedef VectorN,3> Matrix3; typedef VectorN,24> Matrix24; typedef VectorN,24> Matrix24_50; +#else + typedef float Vector2[2]; + typedef float Vector3[3]; + typedef float Vector6[6]; + typedef float Vector8[8]; + typedef float Vector11[11]; + typedef float Vector13[13]; + typedef float Vector21[21]; + typedef float Vector24[24]; + typedef float Matrix3[3][3]; + typedef float Matrix24[24][24]; + typedef float Matrix24_50[24][50]; +#endif // Constructor NavEKF(const AP_AHRS &ahrs, AP_Baro &baro); @@ -141,12 +163,13 @@ private: bool statesInitialised; + Vector24 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 + Matrix24 KH; // intermediate result used for covariance updates Matrix24 KHP; // intermediate result used for covariance updates Matrix24 P; // covariance matrix - Vector24 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 Matrix24_50 storedStates; // state vectors stored for the last 50 time steps - VectorN statetimeStamp; // time stamp for each state vector stored + 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; // corrected & summed delta angles about the xyz body axes (rad) @@ -205,6 +228,12 @@ private: const uint32_t MAGmsecMax; // maximum allowed interval between compass fusion steps uint32_t HGTmsecPrev; // time stamp of last height measurement fusion step const uint32_t HGTmsecMax; // maximum allowed interval between height measurement fusion steps + + // last time compass was updated + uint32_t lastMagUpdate; + + // last time airspeed was updated + uint32_t lastAirspeedUpdate; // Estimated time delays (msec) for different measurements relative to IMU const uint32_t msecVelDelay; @@ -234,11 +263,9 @@ private: uint32_t MAGtime; uint32_t lastMAGtime; bool newDataMag; - Vector3f magDataPrev; // TAS input variables bool newDataTas; - float VtasMeasPrev; // AHRS input data variables Vector3f ahrsEul; @@ -280,6 +307,15 @@ private: Vector3f lastAngRate; Vector3f lastAccel; + + // CovariancePrediction variables + Matrix24 nextP; + Vector24 processNoise; + Vector21 SF; + Vector8 SG; + Vector11 SQ; + Vector13 SPP; + }; #endif // AP_NavEKF