From 577670ccee81c1db37e42447e1994e0c99439bde Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 10 Nov 2015 11:25:44 +1100 Subject: [PATCH] AP_NavEKF2: Reduce memory required by 6KB when running at 400Hz Down-sample the IMU and output observer state data to 100Hz for storage in the buffer. This reduces storage requirements for Copter by 75% or 6KB It does not affect memory required by plane which already uses short buffers due to its 50Hz execution rate. This means that the EKF filter operations operate at a maximum rate of 100Hz. The output observer continues to operate at 400Hz and coning and sculling corrections are applied during the down-sampling so there is no loss of accuracy. --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 16 +- libraries/AP_NavEKF2/AP_NavEKF2.h | 2 +- .../AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp | 4 - libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp | 4 +- .../AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 105 +++++++++---- .../AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp | 2 - libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 15 +- .../AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp | 5 - libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 138 ++++++++---------- libraries/AP_NavEKF2/AP_NavEKF2_core.h | 27 ++-- 11 files changed, 183 insertions(+), 137 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index a2b400ddb8..f94a7c25df 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -434,7 +434,7 @@ NavEKF2::NavEKF2(const AP_AHRS *ahrs, AP_Baro &baro, const RangeFinder &rng) : gndEffectTimeout_ms(1000), // time in msec that baro ground effect compensation will timeout after initiation gndEffectBaroScaler(4.0f), // scaler applied to the barometer observation variance when operating in ground effect gndGradientSigma(2), // RMS terrain gradient percentage assumed by the terrain height estimation - fusionTimeStep_ms(10) // The nominal number of msec between covariance prediction and fusion operations + fusionTimeStep_ms(10) // The minimum number of msec between covariance prediction and fusion operations { AP_Param::setup_object_defaults(this, var_info); } @@ -497,8 +497,20 @@ void NavEKF2::UpdateFilter(void) if (!core) { return; } + + const AP_InertialSensor &ins = _ahrs->get_ins(); + for (uint8_t i=0; i 0) && (core[i-1].getFramesSincePredict() < 2) && (ins.get_sample_rate() > 200)) { + statePredictEnabled = false; + } else { + statePredictEnabled = true; + } + core[i].UpdateFilter(statePredictEnabled); } // If the current core selected has a bad fault score or is unhealthy, switch to a healthy core with the lowest fault score diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index cb20b63e9f..87d92415bd 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -337,7 +337,7 @@ private: const uint16_t gndEffectTimeout_ms; // time in msec that ground effect mode is active after being activated const float gndEffectBaroScaler; // scaler applied to the barometer observation variance when ground effect mode is active const uint8_t gndGradientSigma; // RMS terrain gradient percentage assumed by the terrain height estimation - const uint8_t fusionTimeStep_ms; // The nominal time interval between covariance predictions and measurement fusions in msec + const uint8_t fusionTimeStep_ms; // The minimum time interval between covariance predictions and measurement fusions in msec }; #endif //AP_NavEKF2 diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp index 5a15e33d7c..7d13b4f7de 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp @@ -215,8 +215,6 @@ void NavEKF2_core::SelectTasFusion() tasDataWaiting = (statesInitialised && !inhibitWindStates && newDataTas); if (tasDataWaiting) { - // ensure that the covariance prediction is up to date before fusing data - if (!covPredStep) CovariancePrediction(); FuseAirspeed(); prevTasStep_ms = imuSampleTime_ms; tasDataWaiting = false; @@ -285,8 +283,6 @@ void NavEKF2_core::SelectBetaFusion() bool f_feasible = (assume_zero_sideslip() && !inhibitWindStates); // use synthetic sideslip fusion if feasible, required and enough time has lapsed since the last fusion if (f_feasible && f_required && f_timeTrigger) { - // ensure that the covariance prediction is up to date before fusing data - if (!covPredStep) CovariancePrediction(); FuseSideslip(); prevBetaStep_ms = imuSampleTime_ms; } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index 50a9790154..c66485107c 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp @@ -164,7 +164,7 @@ void NavEKF2_core::setAidingMode() void NavEKF2_core::checkAttitudeAlignmentStatus() { // Check for tilt convergence - used during initial alignment - float alpha = 1.0f*dtIMUavg; + float alpha = 1.0f*imuDataDelayed.delAngDT; float temp=tiltErrVec.length(); tiltErrFilt = alpha*temp + (1.0f-alpha)*tiltErrFilt; if (tiltErrFilt < 0.005f && !tiltAlignComplete) { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp index 18f3762e3d..2e70960849 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp @@ -152,8 +152,6 @@ void NavEKF2_core::SelectMagFusion() // wait until the EKF time horizon catches up with the measurement bool dataReady = (newMagDataAvailable && statesInitialised && use_compass() && yawAlignComplete); if (dataReady) { - // ensure that the covariance prediction is up to date before fusing data - if (!covPredStep) CovariancePrediction(); // If we haven't performed the first airborne magnetic field update or have inhibited magnetic field learning, then we use the simple method of declination to maintain heading if(inhibitMagStates) { fuseCompass(); @@ -261,7 +259,7 @@ void NavEKF2_core::FuseMagnetometer() } // scale magnetometer observation error with total angular rate to allow for timing errors - R_MAG = sq(constrain_float(frontend->_magNoise, 0.01f, 0.5f)) + sq(frontend->magVarRateScale*imuDataDelayed.delAng.length() / dtIMUavg); + R_MAG = sq(constrain_float(frontend->_magNoise, 0.01f, 0.5f)) + sq(frontend->magVarRateScale*imuDataDelayed.delAng.length() / imuDataDelayed.delAngDT); // calculate common expressions used to calculate observation jacobians an innovation variance for each component SH_MAG[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3); diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index bd910fb741..4ba23a6e14 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -112,9 +112,8 @@ void NavEKF2_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRa flowValidMeaTime_ms = imuSampleTime_ms; // estimate sample time of the measurement ofDataNew.time_ms = imuSampleTime_ms - frontend->_flowDelay_ms - frontend->flowTimeDeltaAvg_ms/2; - // Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame - // This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors - ofDataNew.time_ms = roundToNearest(ofDataNew.time_ms, frontend->fusionTimeStep_ms); + // Correct for the average intersampling delay due to the filter updaterate + ofDataNew.time_ms -= localFilterTimeStep_ms/2; // Prevent time delay exceeding age of oldest IMU data in the buffer ofDataNew.time_ms = max(ofDataNew.time_ms,imuDataDelayed.time_ms); // Save data to buffer @@ -224,9 +223,8 @@ void NavEKF2_core::readMagData() // estimate of time magnetometer measurement was taken, allowing for delays magDataNew.time_ms = imuSampleTime_ms - frontend->magDelay_ms; - // Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame - // This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors - magDataNew.time_ms = roundToNearest(magDataNew.time_ms, frontend->fusionTimeStep_ms); + // Correct for the average intersampling delay due to the filter updaterate + magDataNew.time_ms -= localFilterTimeStep_ms/2; // read compass data and scale to improve numerical conditioning magDataNew.mag = _ahrs->get_compass()->get_field(magSelectIndex) * 0.001f; @@ -285,7 +283,13 @@ bool NavEKF2_core::RecallMag() * Inertial Measurements * ********************************************************/ -// update IMU delta angle and delta velocity measurements +/* + * Read IMU delta angle and delta velocity measurements and downsample to 100Hz + * for storage in the data buffers used by the EKF. If the IMU data arrives at + * lower rate than 100Hz, then no downsampling or upsampling will be performed. + * Downsampling is done using a method that does not introduce coning or sculling + * errors. + */ void NavEKF2_core::readIMUData() { const AP_InertialSensor &ins = _ahrs->get_ins(); @@ -311,11 +315,65 @@ void NavEKF2_core::readIMUData() } imuDataNew.delAngDT = max(ins.get_delta_time(),1.0e-4f); - // get current time stamp + // Get current time stamp imuDataNew.time_ms = imuSampleTime_ms; - // save data in the FIFO buffer - StoreIMU(); + // remove gyro scale factor errors + imuDataNew.delAng.x = imuDataNew.delAng.x * stateStruct.gyro_scale.x; + imuDataNew.delAng.y = imuDataNew.delAng.y * stateStruct.gyro_scale.y; + imuDataNew.delAng.z = imuDataNew.delAng.z * stateStruct.gyro_scale.z; + + // remove sensor bias errors + imuDataNew.delAng -= stateStruct.gyro_bias; + imuDataNew.delVel.z -= stateStruct.accel_zbias; + + // Accumulate the measurement time interval for the delta velocity and angle data + imuDataDownSampledNew.delAngDT += imuDataNew.delAngDT; + imuDataDownSampledNew.delVelDT += imuDataNew.delVelDT; + + // Rotate quaternon atitude from previous to new and normalise. + // Accumulation using quaternions prevents introduction of coning errors due to downsampling + Quaternion deltaQuat; + deltaQuat.rotate(imuDataNew.delAng); + imuQuatDownSampleNew = imuQuatDownSampleNew*deltaQuat; + imuQuatDownSampleNew.normalize(); + + // Rotate the accumulated delta velocity into the new frame of reference created by the latest delta angle + // This prevents introduction of sculling errors due to downsampling + Matrix3f deltaRotMat; + deltaQuat.inverse().rotation_matrix(deltaRotMat); + imuDataDownSampledNew.delVel = deltaRotMat*imuDataDownSampledNew.delVel; + + // accumulate the latest delta velocity + imuDataDownSampledNew.delVel += imuDataNew.delVel; + + // 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 >= 0.01f && startPredictEnabled) || (dtIMUavg*(float)framesSincePredict >= 0.02f)) { + // convert the accumulated quaternion to an equivalent delta angle + imuQuatDownSampleNew.to_axis_angle(imuDataDownSampledNew.delAng); + // Time stamp the data + imuDataDownSampledNew.time_ms = imuSampleTime_ms; + // Write data to the FIFO IMU buffer + StoreIMU(); + // zero the accumulated IMU data and quaternion + imuDataDownSampledNew.delAng.zero(); + imuDataDownSampledNew.delVel.zero(); + imuDataDownSampledNew.delAngDT = 0.0f; + imuDataDownSampledNew.delVelDT = 0.0f; + imuQuatDownSampleNew[0] = 1.0f; + imuQuatDownSampleNew[3] = imuQuatDownSampleNew[2] = imuQuatDownSampleNew[1] = 0.0f; + // reset the counter used to let the frontend know how many frames have elapsed since we started a new update cycle + framesSincePredict = 0; + // set the flag to let the filter know it has new IMU data nad needs to run + runUpdates = true; + } else { + // we don't have new IMU data in the buffer so don't run filter updates on this time step + runUpdates = false; + } // extract the oldest available data from the FIFO buffer imuDataDelayed = storedIMU[fifoIndexDelayed]; @@ -330,10 +388,10 @@ void NavEKF2_core::StoreIMU() if (fifoIndexNow >= IMU_BUFFER_LENGTH) { fifoIndexNow = 0; } - storedIMU[fifoIndexNow] = imuDataNew; + storedIMU[fifoIndexNow] = imuDataDownSampledNew; // set the index required to access the oldest data, applying an offset to the fusion time horizon that is used to // prevent the same fusion operation being performed on the same frame across multiple EKF's - fifoIndexDelayed = fifoIndexNow + 1 + fusionHorizonOffset; + fifoIndexDelayed = fifoIndexNow + 1; if (fifoIndexDelayed >= IMU_BUFFER_LENGTH) { fifoIndexDelayed = 0; } @@ -359,7 +417,7 @@ void NavEKF2_core::RecallIMU() imuDataDelayed = storedIMU[fifoIndexDelayed]; // make sure that the delta time used for the delta angles and velocities are is no less than 10% of dtIMUavg to prevent // divide by zero problems when converting to rates or acceleration - float minDT = 0.1f*dtIMUavg; + float minDT = 0.1f*dtEkfAvg; imuDataDelayed.delAngDT = max(imuDataDelayed.delAngDT,minDT); imuDataDelayed.delVelDT = max(imuDataDelayed.delVelDT,minDT); } @@ -401,9 +459,8 @@ void NavEKF2_core::readGpsData() // ideally we should be using a timing signal from the GPS receiver to set this time gpsDataNew.time_ms = lastTimeGpsReceived_ms - frontend->_gpsDelay_ms; - // Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame - // This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors - gpsDataNew.time_ms = roundToNearest(gpsDataNew.time_ms, frontend->fusionTimeStep_ms); + // Correct for the average intersampling delay due to the filter updaterate + gpsDataNew.time_ms -= localFilterTimeStep_ms/2; // Prevent time delay exceeding age of oldest IMU data in the buffer gpsDataNew.time_ms = max(gpsDataNew.time_ms,imuDataDelayed.time_ms); @@ -637,9 +694,8 @@ void NavEKF2_core::readHgtData() // estimate of time height measurement was taken, allowing for delays baroDataNew.time_ms = lastHgtReceived_ms - frontend->_hgtDelay_ms; - // Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame - // This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors - baroDataNew.time_ms = roundToNearest(baroDataNew.time_ms, frontend->fusionTimeStep_ms); + // Correct for the average intersampling delay due to the filter updaterate + baroDataNew.time_ms -= localFilterTimeStep_ms/2; // Prevent time delay exceeding age of oldest IMU data in the buffer baroDataNew.time_ms = max(baroDataNew.time_ms,imuDataDelayed.time_ms); @@ -708,9 +764,8 @@ void NavEKF2_core::readAirSpdData() tasDataNew.tas = aspeed->get_airspeed() * aspeed->get_EAS2TAS(); timeTasReceived_ms = aspeed->last_update_ms(); tasDataNew.time_ms = timeTasReceived_ms - frontend->tasDelay_ms; - // Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame - // This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors - tasDataNew.time_ms = roundToNearest(tasDataNew.time_ms, frontend->fusionTimeStep_ms); + // Correct for the average intersampling delay due to the filter update rate + tasDataNew.time_ms -= localFilterTimeStep_ms/2; newDataTas = true; StoreTAS(); RecallTAS(); @@ -719,10 +774,4 @@ void NavEKF2_core::readAirSpdData() } } -// Round to the nearest multiple of a integer -uint32_t NavEKF2_core::roundToNearest(uint32_t dividend, uint32_t divisor ) -{ - return ((uint32_t)round((float)dividend/float(divisor)))*divisor; -} - #endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp index 11f9dd52ee..a7ca02d9b4 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp @@ -84,8 +84,6 @@ void NavEKF2_core::SelectFlowFusion() { // Set the flow noise used by the fusion processes R_LOS = sq(max(frontend->_flowNoise, 0.05f)); - // ensure that the covariance prediction is up to date before fusing data - if (!covPredStep) CovariancePrediction(); // Fuse the optical flow X and Y axis data into the main filter sequentially FuseOptFlow(); // reset flag to indicate that no new flow data is available for fusion diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index 94b6b44e78..221278bd97 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -104,11 +104,11 @@ void NavEKF2_core::getEulerAngles(Vector3f &euler) const // return body axis gyro bias estimates in rad/sec void NavEKF2_core::getGyroBias(Vector3f &gyroBias) const { - if (dtIMUavg < 1e-6f) { + if (dtEkfAvg < 1e-6f) { gyroBias.zero(); return; } - gyroBias = stateStruct.gyro_bias / dtIMUavg; + gyroBias = stateStruct.gyro_bias / dtEkfAvg; } // return body axis gyro scale factor error as a percentage @@ -198,8 +198,8 @@ void NavEKF2_core::getAccelNED(Vector3f &accelNED) const { // return the Z-accel bias estimate in m/s^2 void NavEKF2_core::getAccelZBias(float &zbias) const { - if (dtIMUavg > 0) { - zbias = stateStruct.accel_zbias / dtIMUavg; + if (dtEkfAvg > 0) { + zbias = stateStruct.accel_zbias / dtEkfAvg; } else { zbias = 0; } @@ -544,4 +544,11 @@ const char *NavEKF2_core::prearm_failure_reason(void) const } +// report the number of frames lapsed since the last state prediction +// this is used by other instances to level load +uint8_t NavEKF2_core::getFramesSincePredict(void) const +{ + return framesSincePredict; +} + #endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp index 424dab8f73..a0ec8758e0 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp @@ -190,8 +190,6 @@ void NavEKF2_core::SelectVelPosFusion() // perform fusion if (fuseVelData || fusePosData || fuseHgtData) { - // ensure that the covariance prediction is up to date before fusing data - if (!covPredStep) CovariancePrediction(); FuseVelPosNED(); // clear the flags to prevent repeated fusion of the same data fuseVelData = false; @@ -233,7 +231,6 @@ void NavEKF2_core::FuseVelPosNED() // so we might as well take advantage of the computational efficiencies // associated with sequential fusion if (fuseVelData || fusePosData || fuseHgtData) { - // set the GPS data timeout depending on whether airspeed data is present uint32_t gpsRetryTime; if (useAirspeed()) gpsRetryTime = frontend->gpsRetryTimeUseTAS_ms; @@ -401,7 +398,6 @@ void NavEKF2_core::FuseVelPosNED() if (fuseHgtData) { // calculate height innovations innovVelPos[5] = stateStruct.position.z - observation[5]; - varInnovVelPos[5] = P[8][8] + R_OBS_DATA_CHECKS[5]; // calculate the innovation consistency test ratio hgtTestRatio = sq(innovVelPos[5]) / (sq(frontend->_hgtInnovGate) * varInnovVelPos[5]); @@ -516,7 +512,6 @@ void NavEKF2_core::FuseVelPosNED() stateStruct.angErr.zero(); // calculate state corrections and re-normalise the quaternions for states predicted using the blended IMU data - // Don't apply corrections to Z bias state as this has been done already as part of the single IMU calculations for (uint8_t i = 0; i<=stateIndexLim; i++) { statesArray[i] = statesArray[i] - Kfusion[i] * innovVelPos[obsIndex]; } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index e26238d1e2..000c1f2c8a 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -71,10 +71,11 @@ void NavEKF2_core::setup_core(NavEKF2 *_frontend, uint8_t _imu_index, uint8_t _c // Use a function call rather than a constructor to initialise variables because it enables the filter to be re-started in flight if necessary. void NavEKF2_core::InitialiseVariables() { - // Offset the fusion horizon if necessary to prevent frame over-runs - if (dtIMUavg < 0.005) { - fusionHorizonOffset = 2*core_index; - } + // calculate the nominal filter update rate + const AP_InertialSensor &ins = _ahrs->get_ins(); + localFilterTimeStep_ms = (uint8_t)(1000.0f/(float)ins.get_sample_rate()); + localFilterTimeStep_ms = max(localFilterTimeStep_ms,10); + // initialise time stamps imuSampleTime_ms = hal.scheduler->millis(); lastHealthyMagTime_ms = imuSampleTime_ms; @@ -113,10 +114,9 @@ void NavEKF2_core::InitialiseVariables() badIMUdata = false; firstMagYawInit = false; dtIMUavg = 0.0025f; + dtEkfAvg = 0.01f; dt = 0; velDotNEDfilt.zero(); - summedDelAng.zero(); - summedDelVel.zero(); lastKnownPositionNE.zero(); prevTnb.zero(); memset(&P[0][0], 0, sizeof(P)); @@ -204,6 +204,12 @@ void NavEKF2_core::InitialiseVariables() velResetNE.zero(); hgtInnovFiltState = 0.0f; magSelectIndex = _ahrs->get_compass()->get_primary(); + imuDataDownSampledNew.delAng.zero(); + imuDataDownSampledNew.delVel.zero(); + imuDataDownSampledNew.delAngDT = 0.0f; + imuDataDownSampledNew.delVelDT = 0.0f; + runUpdates = false; + framesSincePredict = 0; } // Initialise the states from accelerometer and magnetometer data (if present) @@ -221,6 +227,7 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void) // Initialise IMU data dtIMUavg = 1.0f/_ahrs->get_ins().get_sample_rate(); + dtEkfAvg = min(0.01f,dtIMUavg); readIMUData(); StoreIMU_reset(); @@ -311,7 +318,7 @@ void NavEKF2_core::CovarianceInit() P[7][7] = P[6][6]; P[8][8] = sq(frontend->_baroAltNoise); // gyro delta angle biases - P[9][9] = sq(radians(InitialGyroBiasUncertainty() * dtIMUavg)); + P[9][9] = sq(radians(InitialGyroBiasUncertainty() * dtEkfAvg)); P[10][10] = P[9][9]; P[11][11] = P[9][9]; // gyro scale factor biases @@ -319,7 +326,7 @@ void NavEKF2_core::CovarianceInit() P[13][13] = P[12][12]; P[14][14] = P[12][12]; // Z delta velocity bias - P[15][15] = sq(INIT_ACCEL_BIAS_UNCERTAINTY * dtIMUavg); + P[15][15] = sq(INIT_ACCEL_BIAS_UNCERTAINTY * dtEkfAvg); // earth magnetic field P[16][16] = 0.0f; P[17][17] = P[16][16]; @@ -342,8 +349,11 @@ void NavEKF2_core::CovarianceInit() * UPDATE FUNCTIONS * ********************************************************/ // Update Filter States - this should be called whenever new IMU data is available -void NavEKF2_core::UpdateFilter() +void NavEKF2_core::UpdateFilter(bool predict) { + // Set the flag to indicate to the filter that the front-end has given permission for a new state prediction cycle to be started + startPredictEnabled = predict; + // zero the delta quaternion used by the strapdown navigation because it is published // and we need to return a zero rotation of the INS fails to update it correctedDelAngQuat.initialise(); @@ -370,41 +380,33 @@ void NavEKF2_core::UpdateFilter() // read IMU data as delta angles and velocities readIMUData(); - // State Prediction Step - // Run the strapdown INS equations to predict kinematic states forward to the fusion time horizon using buffered IMU data - UpdateStrapdownEquationsNED(); + // Run the EKF equations to estimate at the fusion time horizon if new IMU data is available in the buffer + if (runUpdates) { + // Predict states using IMU data from the delayed time horizon + UpdateStrapdownEquationsNED(); - // sum delta angles and time used by covariance prediction - summedDelAng = summedDelAng + correctedDelAng; - summedDelVel = summedDelVel + correctedDelVel; - dt += imuDataDelayed.delAngDT; - - // perform a covariance prediction if the total delta angle has exceeded the limit - // or the time limit will be exceeded at the next IMU update - if (((dt >= (frontend->covTimeStepMax - dtIMUavg)) || (summedDelAng.length() > frontend->covDelAngMax))) { + // Predict the covariance growth CovariancePrediction(); - } else { - covPredStep = false; + + // Read range finder data which is used by both position and optical flow fusion + readRangeFinder(); + + // Update states using magnetometer data + SelectMagFusion(); + + // Update states using GPS and altimeter data + SelectVelPosFusion(); + + // Update states using optical flow data + SelectFlowFusion(); + + // Update states using airspeed data + SelectTasFusion(); + + // Update states using sideslip constraint assumption for fly-forward vehicles + SelectBetaFusion(); } - // Read range finder data which is used by both position and optical flow fusion - readRangeFinder(); - - // Update states using magnetometer data - SelectMagFusion(); - - // Update states using GPS and altimeter data - SelectVelPosFusion(); - - // Update states using optical flow data - SelectFlowFusion(); - - // Update states using airspeed data - SelectTasFusion(); - - // Update states using sideslip constraint assumption for fly-forward vehicles - SelectBetaFusion(); - // Wind output forward from the fusion to output time horizon calcOutputStatesFast(); @@ -424,21 +426,9 @@ void NavEKF2_core::UpdateFilter() */ void NavEKF2_core::UpdateStrapdownEquationsNED() { - Vector3f delVelNav; // delta velocity vector - - // remove gyro scale factor errors - correctedDelAng.x = imuDataDelayed.delAng.x * stateStruct.gyro_scale.x; - correctedDelAng.y = imuDataDelayed.delAng.y * stateStruct.gyro_scale.y; - correctedDelAng.z = imuDataDelayed.delAng.z * stateStruct.gyro_scale.z; - - // remove sensor bias errors - correctedDelAng -= stateStruct.gyro_bias; - correctedDelVel = imuDataDelayed.delVel; - correctedDelVel.z -= stateStruct.accel_zbias; - // apply correction for earths rotation rate // % * - and + operators have been overloaded - correctedDelAng = correctedDelAng - prevTnb * earthRateNED*imuDataDelayed.delAngDT; + correctedDelAng = imuDataDelayed.delAng - prevTnb * earthRateNED*imuDataDelayed.delAngDT; // convert the rotation vector to its equivalent quaternion correctedDelAngQuat.from_axis_angle(correctedDelAng); @@ -455,7 +445,8 @@ void NavEKF2_core::UpdateStrapdownEquationsNED() // transform body delta velocities to delta velocities in the nav frame // * and + operators have been overloaded - delVelNav = Tbn_temp*correctedDelVel; + Vector3f delVelNav; // delta velocity vector in earth axes + delVelNav = Tbn_temp*imuDataDelayed.delVel; delVelNav.z += GRAVITY_MSS*imuDataDelayed.delVelDT; // calculate the rate of change of velocity (used for launch detect and other functions) @@ -479,8 +470,8 @@ void NavEKF2_core::UpdateStrapdownEquationsNED() stateStruct.position += (stateStruct.velocity + lastVelocity) * (imuDataDelayed.delVelDT*0.5f); // accumulate the bias delta angle and time since last reset by an OF measurement arrival - delAngBodyOF += imuDataNew.delAng - stateStruct.gyro_bias; - delTimeOF += imuDataNew.delAngDT; + delAngBodyOF += imuDataDelayed.delAng - stateStruct.gyro_bias; + delTimeOF += imuDataDelayed.delAngDT; // limit states to protect against divergence ConstrainStates(); @@ -545,8 +536,10 @@ void NavEKF2_core::calcOutputStatesFast() { // apply a trapezoidal integration to velocities to calculate position, applying correction required to track EKF solution outputDataNew.position += (outputDataNew.velocity + lastVelocity) * (imuDataNew.delVelDT*0.5f) + velCorrection * imuDataNew.delVelDT; - // store the output in the FIFO buffer - StoreOutput(); + // store the output in the FIFO buffer if this is a filter update step + if (runUpdates) { + StoreOutput(); + } // extract data at the fusion time horizon from the FIFO buffer RecallOutput(); @@ -630,6 +623,7 @@ void NavEKF2_core::CovariancePrediction() // 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; float alpha = 0.1f * dt; hgtRate = hgtRate * (1.0f - alpha) - stateStruct.velocity.z * alpha; @@ -657,12 +651,12 @@ void NavEKF2_core::CovariancePrediction() for (uint8_t i= 0; i<=stateIndexLim; i++) processNoise[i] = sq(processNoise[i]); // set variables used to calculate covariance growth - dvx = summedDelVel.x; - dvy = summedDelVel.y; - dvz = summedDelVel.z; - dax = summedDelAng.x; - day = summedDelAng.y; - daz = summedDelAng.z; + dvx = imuDataDelayed.delVel.x; + dvy = imuDataDelayed.delVel.y; + dvz = imuDataDelayed.delVel.z; + dax = imuDataDelayed.delAng.x; + day = imuDataDelayed.delAng.y; + daz = imuDataDelayed.delAng.z; q0 = stateStruct.quat[0]; q1 = stateStruct.quat[1]; q2 = stateStruct.quat[2]; @@ -1098,17 +1092,9 @@ void NavEKF2_core::CovariancePrediction() // constrain diagonals to prevent ill-conditioning ConstrainVariances(); - // set the flag to indicate that covariance prediction has been performed and reset the increments used by the covariance prediction - covPredStep = true; - summedDelAng.zero(); - summedDelVel.zero(); - dt = 0.0f; - hal.util->perf_end(_perf_CovariancePrediction); } - - // zero specified range of rows in the state covariance matrix void NavEKF2_core::zeroRows(Matrix24 &covMat, uint8_t first, uint8_t last) { @@ -1218,9 +1204,9 @@ void NavEKF2_core::ConstrainVariances() for (uint8_t i=0; i<=2; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0f); // attitude error for (uint8_t i=3; i<=5; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e3f); // velocities for (uint8_t i=6; i<=8; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e6f); // positions - for (uint8_t i=9; i<=11; i++) P[i][i] = constrain_float(P[i][i],0.0f,sq(0.175f * dtIMUavg)); // delta angle biases + for (uint8_t i=9; i<=11; i++) P[i][i] = constrain_float(P[i][i],0.0f,sq(0.175f * dtEkfAvg)); // delta angle biases for (uint8_t i=12; i<=14; i++) P[i][i] = constrain_float(P[i][i],0.0f,0.01f); // delta angle scale factors - P[15][15] = constrain_float(P[15][15],0.0f,sq(10.0f * dtIMUavg)); // delta velocity bias + P[15][15] = constrain_float(P[15][15],0.0f,sq(10.0f * dtEkfAvg)); // delta velocity bias for (uint8_t i=16; i<=18; i++) P[i][i] = constrain_float(P[i][i],0.0f,0.01f); // earth magnetic field for (uint8_t i=19; i<=21; i++) P[i][i] = constrain_float(P[i][i],0.0f,0.01f); // body magnetic field for (uint8_t i=22; i<=23; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e3f); // wind velocity @@ -1238,11 +1224,11 @@ void NavEKF2_core::ConstrainStates() // height limit covers home alt on everest through to home alt at SL and ballon drop stateStruct.position.z = constrain_float(stateStruct.position.z,-4.0e4f,1.0e4f); // gyro bias limit (this needs to be set based on manufacturers specs) - for (uint8_t i=9; i<=11; i++) statesArray[i] = constrain_float(statesArray[i],-GYRO_BIAS_LIMIT*dtIMUavg,GYRO_BIAS_LIMIT*dtIMUavg); + for (uint8_t i=9; i<=11; i++) statesArray[i] = constrain_float(statesArray[i],-GYRO_BIAS_LIMIT*dtEkfAvg,GYRO_BIAS_LIMIT*dtEkfAvg); // gyro scale factor limit of +-5% (this needs to be set based on manufacturers specs) for (uint8_t i=12; i<=14; i++) statesArray[i] = constrain_float(statesArray[i],0.95f,1.05f); // Z accel bias limit 1.0 m/s^2 (this needs to be finalised from test data) - stateStruct.accel_zbias = constrain_float(stateStruct.accel_zbias,-1.0f*dtIMUavg,1.0f*dtIMUavg); + stateStruct.accel_zbias = constrain_float(stateStruct.accel_zbias,-1.0f*dtEkfAvg,1.0f*dtEkfAvg); // earth magnetic field limit for (uint8_t i=16; i<=18; i++) statesArray[i] = constrain_float(statesArray[i],-1.0f,1.0f); // body magnetic field limit diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 1ddb5d3438..16fb1e5ce5 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -50,7 +50,7 @@ // Note that if using more than 2 instances of the EKF, as set by EK2_IMU_MASK, this delay should be increased by 2 samples // for each additional instance to allow for the need to offset the fusion time horizon for each instance to avoid simultaneous fusion // of measurements by each instance -#define IMU_BUFFER_LENGTH 104 // maximum 260 msec delay at 400 Hz +#define IMU_BUFFER_LENGTH 26 // maximum 260 msec delay at 100 Hz fusion rate #elif APM_BUILD_TYPE(APM_BUILD_APMrover2) #define IMU_BUFFER_LENGTH 13 // maximum 260 msec delay at 50 Hz #elif APM_BUILD_TYPE(APM_BUILD_ArduPlane) @@ -75,7 +75,8 @@ public: bool InitialiseFilterBootstrap(void); // Update Filter States - this should be called whenever new IMU data is available - void UpdateFilter(void); + // The predict flag is set true when a new prediction cycle can be started + void UpdateFilter(bool predict); // Check basic filter health metrics and return a consolidated health status bool healthy(void) const; @@ -272,6 +273,10 @@ public: // report any reason for why the backend is refusing to initialise const char *prearm_failure_reason(void) const; + // report the number of frames lapsed since the last state prediction + // this is used by other instances to level load + uint8_t getFramesSincePredict(void) const; + private: // Reference to the global EKF frontend for parameters NavEKF2 *frontend; @@ -628,12 +633,8 @@ private: // using a simple observer void calcOutputStatesFast(); - // Round to the nearest multiple of a integer - uint32_t roundToNearest(uint32_t dividend, uint32_t divisor ); - // Length of FIFO buffers used for non-IMU sensor data. - // Must be larger than the maximum number of sensor samples that will arrive during the time period defined by IMU_BUFFER_LENGTH - // OBS_BUFFER_LENGTH > IMU_BUFFER_LENGTH * dtIMUavg * 'max sensor rate' + // Must be larger than the time period defined by IMU_BUFFER_LENGTH static const uint32_t OBS_BUFFER_LENGTH = 5; // Variables @@ -665,13 +666,12 @@ private: Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) Quaternion correctedDelAngQuat; // quaternion representation of correctedDelAng Vector3f correctedDelVel; // delta velocities along the XYZ body axes for weighted average of IMU1 and IMU2 corrected for errors (m/s) - Vector3f summedDelAng; // corrected & summed delta angles about the xyz body axes (rad) - Vector3f summedDelVel; // corrected & summed delta velocities along the XYZ body axes (m/s) Matrix3f prevTnb; // previous nav to body transformation used for INS earth rotation compensation ftype accNavMag; // magnitude of navigation accel - used to adjust GPS obs variance (m/s^2) ftype accNavMagHoriz; // magnitude of navigation accel in horizontal plane (m/s^2) Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) ftype dtIMUavg; // expected time between IMU measurements (sec) + ftype dtEkfAvg; // expected time between EKF updates (sec) ftype dt; // time lapsed since the last covariance prediction (sec) ftype hgtRate; // state for rate of change of height filter bool onGround; // true when the flight vehicle is definitely on the ground @@ -689,7 +689,6 @@ private: Vector3f varInnovMag; // innovation variance output from fusion of X,Y,Z compass measurements ftype innovVtas; // innovation output from fusion of airspeed measurements ftype varInnovVtas; // innovation variance output from fusion of airspeed measurements - bool covPredStep; // boolean set to true when a covariance prediction step has been performed bool magFusePerformed; // boolean set to true when magnetometer fusion has been perfomred in that time step bool magFuseRequired; // boolean set to true when magnetometer fusion will be perfomred in the next time step uint32_t prevTasStep_ms; // time stamp of last TAS fusion step @@ -748,6 +747,8 @@ private: uint8_t stateIndexLim; // Max state index used during matrix and array operations imu_elements imuDataDelayed; // IMU data at the fusion time horizon imu_elements imuDataNew; // IMU data at the current time horizon + imu_elements imuDataDownSampledNew; // IMU data at the current time horizon that has been downsampled to a 100Hz rate + Quaternion imuQuatDownSampleNew; // Quaternion obtained by rotating through the IMU delta angles since the start of the current down sampled frame uint8_t fifoIndexNow; // Global index for inertial and output solution at current time horizon uint8_t fifoIndexDelayed; // Global index for inertial and output solution at delayed/fusion time horizon baro_elements baroDataNew; // Baro data at the current time horizon @@ -786,9 +787,13 @@ private: uint32_t lastVelReset_ms; // System time at which the last velocity reset occurred. Returned by getLastVelNorthEastReset float yawTestRatio; // square of magnetometer yaw angle innovation divided by fail threshold Quaternion prevQuatMagReset; // Quaternion from the last time the magnetic field state reset condition test was performed - uint8_t fusionHorizonOffset; // number of IMU samples that the fusion time horizon has been shifted forward to prevent multiple EKF instances fusing data at the same time + uint8_t fusionHorizonOffset; // number of IMU samples that the fusion time horizon has been shifted to prevent multiple EKF instances fusing data at the same time float hgtInnovFiltState; // state used for fitering of the height innovations used for pre-flight checks uint8_t magSelectIndex; // Index of the magnetometer that is being used by the EKF + bool runUpdates; // boolean true when the EKF updates can be run + uint32_t framesSincePredict; // number of frames lapsed since EKF instance did a state prediction + bool startPredictEnabled; // boolean true when the frontend has given permission to start a new state prediciton cycele + uint8_t localFilterTimeStep_ms; // average number of msec between filter updates // variables used to calulate a vertical velocity that is kinematically consistent with the verical position float posDownDerivative; // Rate of chage of vertical position (dPosD/dt) in m/s. This is the first time derivative of PosD.