Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 16:23:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Improve calculation and use of average EKF time step

Browse Source
This commit is contained in:
priseborough 2017-04-28 16:36:16 +10:00 committed by Andrew Tridgell
parent 7abf9997e6
commit 3ce81967ae
3 changed files with 22 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -347,9 +347,12 @@ void NavEKF3_core::readIMUData()
// Keep track of the number of IMU frames since the last state prediction // Keep track of the number of IMU frames since the last state prediction
framesSincePredict++; 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 the target EKF time step has been accumulated, and the frontend has allowed start of a new predict cycle,
if ((dtIMUavg*(float)framesSincePredict >= EKF_TARGET_DT && startPredictEnabled) || (dtIMUavg*(float)framesSincePredict >= 2.0f*EKF_TARGET_DT)) { * 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 // convert the accumulated quaternion to an equivalent delta angle
imuQuatDownSampleNew.to_axis_angle(imuDataDownSampledNew.delAng); imuQuatDownSampleNew.to_axis_angle(imuDataDownSampledNew.delAng);
@ -360,8 +363,8 @@ void NavEKF3_core::readIMUData()
// Write data to the FIFO IMU buffer // Write data to the FIFO IMU buffer
storedIMU.push_youngest_element(imuDataDownSampledNew); storedIMU.push_youngest_element(imuDataDownSampledNew);
// calculate the achieved average time step rate for the EKF // 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.0f,10.0f*EKF_TARGET_DT); float dtNow = constrain_float(0.5f*(imuDataDownSampledNew.delAngDT+imuDataDownSampledNew.delVelDT),0.5f * dtEkfAvg, 2.0f * dtEkfAvg);
dtEkfAvg = 0.98f * dtEkfAvg + 0.02f * dtNow; dtEkfAvg = 0.98f * dtEkfAvg + 0.02f * dtNow;
// zero the accumulated IMU data and quaternion // zero the accumulated IMU data and quaternion
@ -382,8 +385,7 @@ void NavEKF3_core::readIMUData()
imuDataDelayed = storedIMU.pop_oldest_element(); imuDataDelayed = storedIMU.pop_oldest_element();
// protect against delta time going to zero // 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.delAngDT = MAX(imuDataDelayed.delAngDT,minDT);
imuDataDelayed.delVelDT = MAX(imuDataDelayed.delVelDT,minDT); imuDataDelayed.delVelDT = MAX(imuDataDelayed.delVelDT,minDT);

18
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -67,7 +67,7 @@ bool NavEKF3_core::setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _c
MAX(_frontend->_flowDelay_ms , MAX(_frontend->_flowDelay_ms ,
MAX(_frontend->_rngBcnDelay_ms , MAX(_frontend->_rngBcnDelay_ms ,
MAX(_frontend->magDelay_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 // 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 // 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 // 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 // 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 // calculate the nominal filter update rate
const AP_InertialSensor &ins = _ahrs->get_ins(); const AP_InertialSensor &ins = _ahrs->get_ins();
localFilterTimeStep_ms = (uint8_t)(1000*ins.get_loop_delta_t()); 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 // initialise time stamps
imuSampleTime_ms = frontend->imuSampleTime_us / 1000; 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 dvy_b; // Y axis delta velocity measurement bias (rad)
float dvz_b; // Z 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 // use filtered height rate to increase wind process noise when climbing or descending
// this allows for wind gradient effects. // this allows for wind gradient effects.Filter height rate using a 10 second time constant filter
// filter height rate using a 10 second time constant filter
dt = imuDataDelayed.delAngDT;
float alpha = 0.1f * dt; float alpha = 0.1f * dt;
hgtRate = hgtRate * (1.0f - alpha) - stateStruct.velocity.z * alpha; hgtRate = hgtRate * (1.0f - alpha) - stateStruct.velocity.z * alpha;
// use filtered height rate to increase wind process noise when climbing or descending // calculate covariance prediction process noise
// this allows for wind gradient effects.
windVelSigma = dt * constrain_float(frontend->_windVelProcessNoise, 0.0f, 1.0f) * (1.0f + constrain_float(frontend->_wndVarHgtRateScale, 0.0f, 1.0f) * fabsf(hgtRate)); 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); dAngBiasSigma = sq(dt) * constrain_float(frontend->_gyroBiasProcessNoise, 0.0f, 1.0f);
dVelBiasSigma = sq(dt) * constrain_float(frontend->_accelBiasProcessNoise, 0.0f, 1.0f); dVelBiasSigma = sq(dt) * constrain_float(frontend->_accelBiasProcessNoise, 0.0f, 1.0f);

6
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -58,6 +58,10 @@
// initial accel bias uncertainty as a fraction of the state limit // initial accel bias uncertainty as a fraction of the state limit
#define ACCEL_BIAS_LIM_SCALER 0.2f #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 AP_AHRS;
class NavEKF3_core class NavEKF3_core
@ -782,8 +786,6 @@ private:
bool badMagYaw; // boolean true if the magnetometer is declared to be producing bad data 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 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 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 Vector28 Kfusion; // Kalman gain vector
Matrix24 KH; // intermediate result used for covariance updates Matrix24 KH; // intermediate result used for covariance updates