diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 546892e908..7cfab86453 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -347,9 +347,12 @@ void NavEKF3_core::readIMUData() // Keep track of the number of IMU frames since the last state prediction framesSincePredict++; - // If 10msec has elapsed, and the frontend has allowed us to start a new predict cycle, then store the accumulated IMU data - // to be used by the state prediction, ignoring the frontend permission if more than 20msec has lapsed - if ((dtIMUavg*(float)framesSincePredict >= EKF_TARGET_DT && startPredictEnabled) || (dtIMUavg*(float)framesSincePredict >= 2.0f*EKF_TARGET_DT)) { + /* + * If the target EKF time step has been accumulated, and the frontend has allowed start of a new predict cycle, + * then store the accumulated IMU data to be used by the state prediction, ignoring the frontend permission if more + * than twice the target time has lapsed. + */ + if ((imuDataDownSampledNew.delAngDT >= EKF_TARGET_DT && startPredictEnabled) || (imuDataDownSampledNew.delAngDT >= 2.0f*EKF_TARGET_DT)) { // convert the accumulated quaternion to an equivalent delta angle imuQuatDownSampleNew.to_axis_angle(imuDataDownSampledNew.delAng); @@ -360,8 +363,8 @@ void NavEKF3_core::readIMUData() // Write data to the FIFO IMU buffer storedIMU.push_youngest_element(imuDataDownSampledNew); - // calculate the achieved average time step rate for the EKF - float dtNow = constrain_float(0.5f*(imuDataDownSampledNew.delAngDT+imuDataDownSampledNew.delVelDT),0.0f,10.0f*EKF_TARGET_DT); + // calculate the achieved average time step rate for the EKF using a combination spike and LPF + float dtNow = constrain_float(0.5f*(imuDataDownSampledNew.delAngDT+imuDataDownSampledNew.delVelDT),0.5f * dtEkfAvg, 2.0f * dtEkfAvg); dtEkfAvg = 0.98f * dtEkfAvg + 0.02f * dtNow; // zero the accumulated IMU data and quaternion @@ -382,8 +385,7 @@ void NavEKF3_core::readIMUData() imuDataDelayed = storedIMU.pop_oldest_element(); // protect against delta time going to zero - // TODO - check if calculations can tolerate 0 - float minDT = 0.1f*dtEkfAvg; + float minDT = 0.1f * dtEkfAvg; imuDataDelayed.delAngDT = MAX(imuDataDelayed.delAngDT,minDT); imuDataDelayed.delVelDT = MAX(imuDataDelayed.delVelDT,minDT); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index feb7637e0c..ac58057b22 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -67,7 +67,7 @@ bool NavEKF3_core::setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _c MAX(_frontend->_flowDelay_ms , MAX(_frontend->_rngBcnDelay_ms , MAX(_frontend->magDelay_ms , - (uint16_t)(dtEkfAvg*1000.0f) + (uint16_t)(EKF_TARGET_DT_MS) )))); // GPS sensing can have large delays and should not be included if disabled @@ -97,7 +97,7 @@ bool NavEKF3_core::setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _c } // calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter - imu_buffer_length = (maxTimeDelay_ms / (uint16_t)(dtEkfAvg*1000.0f)) + 1; + imu_buffer_length = (maxTimeDelay_ms / (uint16_t)(EKF_TARGET_DT_MS)) + 1; // set the observaton buffer length to handle the minimum time of arrival between observations in combination // with the worst case delay from current time to ekf fusion time @@ -155,7 +155,7 @@ void NavEKF3_core::InitialiseVariables() // calculate the nominal filter update rate const AP_InertialSensor &ins = _ahrs->get_ins(); localFilterTimeStep_ms = (uint8_t)(1000*ins.get_loop_delta_t()); - localFilterTimeStep_ms = MAX(localFilterTimeStep_ms,10); + localFilterTimeStep_ms = MAX(localFilterTimeStep_ms, (uint8_t)EKF_TARGET_DT_MS); // initialise time stamps imuSampleTime_ms = frontend->imuSampleTime_us / 1000; @@ -849,16 +849,16 @@ void NavEKF3_core::CovariancePrediction() float dvy_b; // Y axis delta velocity measurement bias (rad) float dvz_b; // Z axis delta velocity measurement bias (rad) - // calculate covariance prediction process noise + // Calculate the time step used by the covariance prediction as an average of the gyro and accel integration period + // Constrain to prevent bad timing jitter causing numerical conditioning problems with the covariance prediction + dt = constrain_float(0.5f*(imuDataDelayed.delAngDT+imuDataDelayed.delVelDT),0.5f * dtEkfAvg, 2.0f * dtEkfAvg); + // use filtered height rate to increase wind process noise when climbing or descending - // this allows for wind gradient effects. - // filter height rate using a 10 second time constant filter - dt = imuDataDelayed.delAngDT; + // this allows for wind gradient effects.Filter height rate using a 10 second time constant filter float alpha = 0.1f * dt; hgtRate = hgtRate * (1.0f - alpha) - stateStruct.velocity.z * alpha; - // use filtered height rate to increase wind process noise when climbing or descending - // this allows for wind gradient effects. + // calculate covariance prediction process noise windVelSigma = dt * constrain_float(frontend->_windVelProcessNoise, 0.0f, 1.0f) * (1.0f + constrain_float(frontend->_wndVarHgtRateScale, 0.0f, 1.0f) * fabsf(hgtRate)); dAngBiasSigma = sq(dt) * constrain_float(frontend->_gyroBiasProcessNoise, 0.0f, 1.0f); dVelBiasSigma = sq(dt) * constrain_float(frontend->_accelBiasProcessNoise, 0.0f, 1.0f); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index fb35972ab5..6201a96537 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -58,6 +58,10 @@ // initial accel bias uncertainty as a fraction of the state limit #define ACCEL_BIAS_LIM_SCALER 0.2f +// target update time for the EKF in msec and sec +#define EKF_TARGET_DT_MS 10.0f +#define EKF_TARGET_DT 0.01f + class AP_AHRS; class NavEKF3_core @@ -782,8 +786,6 @@ private: bool badMagYaw; // boolean true if the magnetometer is declared to be producing bad data bool badIMUdata; // boolean true if the bad IMU data is detected - const float EKF_TARGET_DT = 0.01f; // target EKF update time step - float gpsNoiseScaler; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts Vector28 Kfusion; // Kalman gain vector Matrix24 KH; // intermediate result used for covariance updates