diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index e89c0dd054..741fd5e914 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -38,9 +38,9 @@ #define ABIAS_PNOISE_DEFAULT 0.0001f #define MAGE_PNOISE_DEFAULT 0.0003f #define MAGB_PNOISE_DEFAULT 0.0003f -#define VEL_GATE_DEFAULT 2 +#define VEL_GATE_DEFAULT 6 #define POS_GATE_DEFAULT 10 -#define HGT_GATE_DEFAULT 5 +#define HGT_GATE_DEFAULT 10 #define MAG_GATE_DEFAULT 3 #define MAG_CAL_DEFAULT 1 #define GLITCH_ACCEL_DEFAULT 150 @@ -59,9 +59,9 @@ #define ABIAS_PNOISE_DEFAULT 0.0002f #define MAGE_PNOISE_DEFAULT 0.0003f #define MAGB_PNOISE_DEFAULT 0.0003f -#define VEL_GATE_DEFAULT 2 +#define VEL_GATE_DEFAULT 6 #define POS_GATE_DEFAULT 10 -#define HGT_GATE_DEFAULT 5 +#define HGT_GATE_DEFAULT 10 #define MAG_GATE_DEFAULT 3 #define MAG_CAL_DEFAULT 1 #define GLITCH_ACCEL_DEFAULT 150 @@ -80,9 +80,9 @@ #define ABIAS_PNOISE_DEFAULT 0.0002f #define MAGE_PNOISE_DEFAULT 0.0003f #define MAGB_PNOISE_DEFAULT 0.0003f -#define VEL_GATE_DEFAULT 2 +#define VEL_GATE_DEFAULT 6 #define POS_GATE_DEFAULT 10 -#define HGT_GATE_DEFAULT 10 +#define HGT_GATE_DEFAULT 20 #define MAG_GATE_DEFAULT 3 #define MAG_CAL_DEFAULT 0 #define GLITCH_ACCEL_DEFAULT 150 @@ -346,6 +346,7 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) : _gyroBiasNoiseScaler = 2.0f; // scale factor applied to gyro bias state process noise when on ground _msecGpsAvg = 200; // average number of msec between GPS measurements _msecHgtAvg = 100; // average number of msec between height measurements + _msecMagAvg = 100; // average number of msec between magnetometer measurements _msecBetaAvg = 100; // average number of msec between synthetic sideslip measurements dtVelPos = 0.02; // number of seconds between position and velocity corrections. This should be a multiple of the imu update interval. // Misc initial conditions @@ -353,7 +354,6 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) : mag_state.q0 = 1; mag_state.DCM.identity(); IMU1_weighting = 0.5f; - lastDivergeTime_ms = 0; memset(&faultStatus, 0, sizeof(faultStatus)); } @@ -411,6 +411,8 @@ void NavEKF::ResetPosition(void) states[7] = gpsPosNE.x + gpsPosGlitchOffsetNE.x; states[8] = gpsPosNE.y + gpsPosGlitchOffsetNE.y; } + // reset the glitch ofset correction states + gpsPosGlitchOffsetNE.zero(); } // resets velocity states to last GPS measurement or to zero if in static mode @@ -464,7 +466,15 @@ void NavEKF::InitialiseFilterDynamic(void) ZeroVariables(); // get initial time deltat between IMU measurements (sec) - dtIMU = _ahrs->get_ins().get_delta_time(); + dtIMU = constrain_float(_ahrs->get_ins().get_delta_time(),0.001f,1.0f); + + // set number of updates over which gps and baro measurements are applied to the velocity and position states + gpsUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecGpsAvg); + gpsUpdateCountMax = uint8_t(1.0f/gpsUpdateCountMaxInv); + hgtUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecHgtAvg); + hgtUpdateCountMax = uint8_t(1.0f/hgtUpdateCountMaxInv); + magUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecMagAvg); + magUpdateCountMax = uint8_t(1.0f/magUpdateCountMaxInv); // calculate initial orientation and earth magnetic field states Quaternion initQuat; @@ -501,6 +511,17 @@ void NavEKF::InitialiseFilterBootstrap(void) // set re-used variables to zero ZeroVariables(); + // get initial time deltat between IMU measurements (sec) + dtIMU = constrain_float(_ahrs->get_ins().get_delta_time(),0.001f,1.0f); + + // set number of updates over which gps and baro measurements are applied to the velocity and position states + gpsUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecGpsAvg); + gpsUpdateCountMax = uint8_t(1.0f/gpsUpdateCountMaxInv); + hgtUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecHgtAvg); + hgtUpdateCountMax = uint8_t(1.0f/hgtUpdateCountMaxInv); + magUpdateCountMaxInv = (dtIMU * 1000.0f)/float(_msecMagAvg); + magUpdateCountMax = uint8_t(1.0f/magUpdateCountMaxInv); + // acceleration vector in XYZ body axes measured by the IMU (m/s^2) Vector3f initAccVec; @@ -651,17 +672,6 @@ void NavEKF::UpdateFilter() // select fusion of velocity, position and height measurements void NavEKF::SelectVelPosFusion() { - // calculate ratio of VelPos fusion to state prediction steps - uint8_t velPosFuseStepRatio = floor(dtVelPos/dtIMU + 0.5f); - - // calculate the scale factor to be applied to GPS measurement variance to account for - // the fact we repeat fusion of the same measurement to provide a smoother output - gpsVarScaler = _msecGpsAvg/(1000.0f*dtVelPos); - - // calculate the scale factor to be applied to height measurement variance to account for - // the fact we repeat fusion of the same measurement to provide a smoother output - hgtVarScaler = _msecHgtAvg/(1000.0f*dtVelPos); - // check for new data, specify which measurements should be used and check data for freshness if (!staticMode) { @@ -672,36 +682,38 @@ void NavEKF::SelectVelPosFusion() if (newDataGps) { // reset data arrived flag newDataGps = false; + // reset state updates and counter used to spread fusion updates across several frames to reduce 10Hz pulsing + memset(&gpsIncrStateDelta[0], 0, sizeof(gpsIncrStateDelta)); + gpsUpdateCount = 0; // enable fusion fuseVelData = true; fusePosData = true; - // reset the counter used to schedule updates so that we always fuse data on the frame GPS data arrives - skipCounter = velPosFuseStepRatio; // If a long time since last GPS update, then reset position and velocity and reset stored state history - uint32_t gpsRetryTimeout = useAirspeed() ? _gpsRetryTimeUseTAS : _gpsRetryTimeNoTAS; if (imuSampleTime_ms - secondLastFixTime_ms > gpsRetryTimeout) { ResetPosition(); ResetVelocity(); StoreStatesReset(); } - } else if (imuSampleTime_ms - lastFixTime_ms > (uint32_t)(_msecGpsAvg + 40)) { - // Timeout fusion of GPS data if stale. Needed because we repeatedly fuse the same - // measurement until the next one arrives to provide a smoother output + } else { fuseVelData = false; fusePosData = false; } + // check for and read new height data + readHgtData(); + // command fusion of height data if (newDataHgt) { // reset data arrived flag newDataHgt = false; + // reset state updates and counter used to spread fusion updates across several frames to reduce 10Hz pulsing + memset(&hgtIncrStateDelta[0], 0, sizeof(hgtIncrStateDelta)); + hgtUpdateCount = 0; // enable fusion fuseHgtData = true; - } else if (imuSampleTime_ms - lastHgtTime_ms > (uint32_t)(_msecHgtAvg + 40)) { - // timeout fusion of height data if stale. Needed because we repeatedly fuse the same - // measurement until the next one arrives to provide a smoother output + } else { fuseHgtData = false; } @@ -718,22 +730,24 @@ void NavEKF::SelectVelPosFusion() fuseHgtData = true; } - // check for and read new height data - readHgtData(); - - // perform fusion as commanded, and in accordance with specified time intervals + // perform fusion if (fuseVelData || fusePosData || fuseHgtData) { - // skip fusion as required to maintain ~dtVelPos time interval between corrections - if (skipCounter >= velPosFuseStepRatio) { FuseVelPosNED(); - // reset counter used to skip update frames - skipCounter = 1; - } else { - // increment counter used to skip update frames - skipCounter += 1; - } } + // Fuse corrections to quaternion, position and velocity states across several time steps to reduce 5 and 10Hz pulsing in the output + if (gpsUpdateCount < gpsUpdateCountMax) { + gpsUpdateCount ++; + for (uint8_t i = 0; i <= 9; i++) { + states[i] += gpsIncrStateDelta[i]; + } + } + if (hgtUpdateCount < hgtUpdateCountMax) { + hgtUpdateCount ++; + for (uint8_t i = 0; i <= 9; i++) { + states[i] += hgtIncrStateDelta[i]; + } + } } // select fusion of magnetometer data @@ -762,8 +776,10 @@ void NavEKF::SelectMagFusion() bool dataReady = statesInitialised && use_compass() && newDataMag; if (dataReady) { - MAGmsecPrev = imuSampleTime_ms; fuseMagData = true; + // reset state updates and counter used to spread fusion updates across several frames to reduce 10Hz pulsing + memset(&magIncrStateDelta[0], 0, sizeof(magIncrStateDelta)); + magUpdateCount = 0; } else { @@ -772,6 +788,15 @@ void NavEKF::SelectMagFusion() // call the function that performs fusion of magnetometer data FuseMagnetometer(); + + // Fuse corrections to quaternion, position and velocity states across several time steps to reduce 10Hz pulsing in the output + if (magUpdateCount < magUpdateCountMax) { + magUpdateCount ++; + for (uint8_t i = 0; i <= 9; i++) { + states[i] += magIncrStateDelta[i]; + } + } + } } @@ -1644,20 +1669,20 @@ void NavEKF::FuseVelPosNED() posErr = _gpsPosVarAccScale * accNavMag; // estimate the GPS Velocity, GPS horiz position and height measurement variances. - R_OBS[0] = gpsVarScaler*(sq(constrain_float(_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(NEvelErr)); + R_OBS[0] = sq(constrain_float(_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(NEvelErr); R_OBS[1] = R_OBS[0]; - R_OBS[2] = gpsVarScaler*(sq(constrain_float(_gpsVertVelNoise, 0.05f, 5.0f)) + sq(DvelErr)); - R_OBS[3] = gpsVarScaler*(sq(constrain_float(_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr)); + R_OBS[2] = sq(constrain_float(_gpsVertVelNoise, 0.05f, 5.0f)) + sq(DvelErr); + R_OBS[3] = sq(constrain_float(_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr); R_OBS[4] = R_OBS[3]; - R_OBS[5] = hgtVarScaler*sq(constrain_float(_baroAltNoise, 0.1f, 10.0f)); + R_OBS[5] = sq(constrain_float(_baroAltNoise, 0.1f, 10.0f)); // if vertical GPS velocity data is being used, check to see if the GPS vertical velocity and barometer // innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting // the accelerometers and we should disable the GPS and barometer innovation consistency checks. bool badIMUdata = false; - if (_fusionModeGPS == 0 && fuseVelData && fuseHgtData) { + if (_fusionModeGPS == 0 && fuseVelData && (imuSampleTime_ms - lastHgtTime_ms) < (2 * _msecHgtAvg)) { // calculate innovations for height and vertical GPS vel measurements - float hgtErr = statesAtVelTime.position.z - observation[5]; + float hgtErr = statesAtHgtTime.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]))) { @@ -1893,9 +1918,21 @@ void NavEKF::FuseVelPosNED() } } - // calculate state corrections and re-normalise the quaternions for blended IMU data predicted states + // calculate state corrections and re-normalise the quaternions for states predicted using the blended IMU data + // attitude, velocity and position corrections are spread across multiple prediction cycles between now + // and the anticipated time for the next measurement. + // Don't spread quaternion corrections if total angle change across predicted interval is going to exceed 0.1 rad + bool highRates = ((gpsUpdateCountMax * correctedDelAng.length()) > 0.1f); for (uint8_t i = 0; i<=21; i++) { - states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex]; + if ((i <= 3 && highRates) || i >= 10 || staticMode) { + states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex]; + } else { + if (obsIndex == 5) { + hgtIncrStateDelta[i] -= Kfusion[i] * innovVelPos[obsIndex] * hgtUpdateCountMaxInv; + } else { + gpsIncrStateDelta[i] -= Kfusion[i] * innovVelPos[obsIndex] * gpsUpdateCountMaxInv; + } + } } state.quat.normalize(); @@ -1916,7 +1953,7 @@ void NavEKF::FuseVelPosNED() } } - // force the covariance matrix to me symmetrical and limit the variances to prevent ill-condiioning. + // force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning. ForceSymmetry(); ConstrainVariances(); @@ -2239,13 +2276,24 @@ void NavEKF::FuseMagnetometer() // Don't fuse unless all componenets pass. The exception is if the bad health has timed out and we are not a fly forward vehicle // In this case we might as well try using the magnetometer, but with a reduced weighting if (magHealth || ((magTestRatio[obsIndex] < 1.0f) && !assume_zero_sideslip() && magTimeout)) { - // correct the state vector + // Attitude, velocity and position corrections are averaged across multiple prediction cycles between now and the anticipated time for the next measurement. + // Don't do averaging of quaternion state corrections if total angle change across predicted interval is going to exceed 0.1 rad + bool highRates = ((magUpdateCountMax * correctedDelAng.length()) > 0.1f); + // Calculate the number of averaging frames left to go. This is required becasue magnetometer fusion is applied across three consecutive prediction cycles + // There is no point averaging if the number of cycles left is less than 2 + float minorFramesToGo = float(magUpdateCountMax) - float(magUpdateCount); + // correct the state vector or store corrections to be applied incrementally for (uint8_t j= 0; j<=21; j++) { // If we are forced to use a bad compass, we reduce the weighting by a factor of 4 if (!magHealth) { Kfusion[j] *= 0.25f; } - states[j] = states[j] - Kfusion[j] * innovMag[obsIndex]; + if ((j <= 3 && highRates) || j >= 10 || staticMode || minorFramesToGo < 1.5f ) { + states[j] = states[j] - Kfusion[j] * innovMag[obsIndex]; + } else { + // scale the correction based on the number of averaging frames left to go + magIncrStateDelta[j] -= Kfusion[j] * innovMag[obsIndex] * (magUpdateCountMaxInv * float(magUpdateCountMax) / minorFramesToGo); + } } // normalise the quaternion states state.quat.normalize(); @@ -3356,6 +3404,10 @@ void NavEKF::ZeroVariables() memset(&processNoise[0], 0, sizeof(processNoise)); memset(&storedStates[0], 0, sizeof(storedStates)); memset(&statetimeStamp[0], 0, sizeof(statetimeStamp)); + memset(&gpsIncrStateDelta[0], 0, sizeof(gpsIncrStateDelta)); + memset(&hgtIncrStateDelta[0], 0, sizeof(hgtIncrStateDelta)); + memset(&magIncrStateDelta[0], 0, sizeof(magIncrStateDelta)); + gpsPosGlitchOffsetNE.zero(); } // return true if we should use the airspeed sensor diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index e628f490ec..5e7e68bc2e 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -347,11 +347,11 @@ private: float _magVarRateScale; // scale factor applied to magnetometer variance due to angular rate uint16_t _msecGpsAvg; // average number of msec between GPS measurements uint16_t _msecHgtAvg; // average number of msec between height measurements + uint16_t _msecMagAvg; // average number of msec between magnetometer measurements uint16_t _msecBetaAvg; // maximum number of msec between synthetic sideslip measurements float dtVelPos; // average of msec between position and velocity corrections // Variables - uint8_t skipCounter; // counter used to skip position and height corrections to achieve _skipRatio bool statesInitialised; // boolean true when filter states have been initialised bool velHealth; // boolean true if velocity measurements have passed innovation consistency check bool posHealth; // boolean true if position measurements have passed innovation consistency check @@ -422,8 +422,6 @@ private: uint32_t TASmsecPrev; // time stamp of last TAS fusion step uint32_t BETAmsecPrev; // time stamp of last synthetic sideslip fusion step const uint32_t TASmsecMax; // maximum allowed interval between TAS fusion steps - uint32_t MAGmsecPrev; // time stamp of last compass fusion step - uint32_t HGTmsecPrev; // time stamp of last height measurement fusion step const bool fuseMeNow; // boolean to force fusion whenever data arrives bool staticMode; // boolean to force position and velocity measurements to zero for pre-arm or bench testing bool prevStaticMode; // value of static mode from last update @@ -436,13 +434,11 @@ private: ftype gpsGndSpd; // GPS ground speed (m/s) bool newDataGps; // true when new GPS data has arrived bool newDataMag; // true when new magnetometer data has arrived - float gpsVarScaler; // scaler applied to gps measurement variance to allow for oversampling bool newDataTas; // true when new airspeed data has arrived bool tasDataWaiting; // true when new airspeed data is waiting to be fused bool newDataHgt; // true when new height data has arrived uint32_t lastHgtMeasTime; // time of last height measurement used to determine if new data has arrived uint32_t lastHgtTime_ms; // time of last height update (msec) used to calculate timeout - float hgtVarScaler; // scaler applied to height measurement variance to allow for oversampling uint32_t velFailTime; // time stamp when GPS velocity measurement last failed covaraiance consistency check (msec) uint32_t posFailTime; // time stamp when GPS position measurement last failed covaraiance consistency check (msec) uint32_t hgtFailTime; // time stamp when height measurement last failed covaraiance consistency check (msec) @@ -471,6 +467,21 @@ private: float tasTestRatio; // sum of squares of true airspeed innovation divided by fail threshold bool inhibitWindStates; // true when wind states and covariances are to remain constant bool inhibitMagStates; // true when magnetic field states and covariances are to remain constant + + // Used by smoothing of state corrections + float gpsIncrStateDelta[10]; // vector of corrections to attitude, velocity and position to be applied over the period between the current and next GPS measurement + float hgtIncrStateDelta[10]; // vector of corrections to attitude, velocity and position to be applied over the period between the current and next height measurement + float magIncrStateDelta[10]; // vector of corrections to attitude, velocity and position to be applied over the period between the current and next magnetometer measurement + uint8_t gpsUpdateCount; // count of the number of minor state corrections using GPS data + uint8_t gpsUpdateCountMax; // limit on the number of minor state corrections using GPS data + float gpsUpdateCountMaxInv; // floating point inverse of gpsFilterCountMax + uint8_t hgtUpdateCount; // count of the number of minor state corrections using Baro data + uint8_t hgtUpdateCountMax; // limit on the number of minor state corrections using Baro data + float hgtUpdateCountMaxInv; // floating point inverse of hgtFilterCountMax + uint8_t magUpdateCount; // count of the number of minor state corrections using Magnetometer data + uint8_t magUpdateCountMax; // limit on the number of minor state corrections using Magnetometer data + float magUpdateCountMaxInv; // floating point inverse of magFilterCountMax + struct { bool diverged:1; bool large_covarience:1;