From 55dee347e0de1d423023e2d44e6fdb51fda13be7 Mon Sep 17 00:00:00 2001 From: priseborough Date: Mon, 4 Jul 2016 16:19:13 +1000 Subject: [PATCH] AP_NavEKF2: Fix bug in averaged filter delta time Average EKF time delta was not being updated. --- libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 6 +++++- libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 3 +-- libraries/AP_NavEKF2/AP_NavEKF2_core.h | 2 ++ 3 files changed, 8 insertions(+), 3 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index 826208e5ee..acf0c4b29a 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -281,7 +281,7 @@ void NavEKF2_core::readIMUData() // 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)) { + if ((dtIMUavg*(float)framesSincePredict >= EKF_TARGET_DT && startPredictEnabled) || (dtIMUavg*(float)framesSincePredict >= 2.0f*EKF_TARGET_DT)) { // convert the accumulated quaternion to an equivalent delta angle imuQuatDownSampleNew.to_axis_angle(imuDataDownSampledNew.delAng); @@ -292,6 +292,10 @@ void NavEKF2_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); + dtEkfAvg = 0.98f * dtEkfAvg + 0.02f * dtNow; + // zero the accumulated IMU data and quaternion imuDataDownSampledNew.delAng.zero(); imuDataDownSampledNew.delVel.zero(); diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index a7d8db10d2..dc59263e5f 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -156,7 +156,7 @@ void NavEKF2_core::InitialiseVariables() finalInflightYawInit = false; finalInflightMagInit = false; dtIMUavg = 0.0025f; - dtEkfAvg = 0.01f; + dtEkfAvg = EKF_TARGET_DT; dt = 0; velDotNEDfilt.zero(); lastKnownPositionNE.zero(); @@ -287,7 +287,6 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void) // Initialise IMU data dtIMUavg = _ahrs->get_ins().get_loop_delta_t(); - dtEkfAvg = MIN(0.01f,dtIMUavg); readIMUData(); storedIMU.reset_history(imuDataNew); imuDataDelayed = imuDataNew; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 9df05e0d27..15739c7a3b 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -646,6 +646,8 @@ 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