From 628d1403192e5ddc859a6c9e0aa86f6ba3390d08 Mon Sep 17 00:00:00 2001 From: priseborough Date: Sun, 21 May 2017 12:15:06 +1000 Subject: [PATCH] AP_NavEKF3: Make target covariance time step larger The target covariance time step has been increased from 10 to 12 msec to improve conditioning of the covariance prediction calculation. --- libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index d511b10e72..80789fdaf0 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -353,7 +353,7 @@ void NavEKF3_core::readIMUData() * than twice the target time has lapsed. Adjust the target EKF step time threshold to allow for timing jitter in the * IMU data. */ - if ((imuDataDownSampledNew.delAngDT >= (EKF_TARGET_DT-(dtIMUavg*0.5)) && startPredictEnabled) || + if ((imuDataDownSampledNew.delAngDT >= (EKF_TARGET_DT-(dtIMUavg*0.5f)) && startPredictEnabled) || (imuDataDownSampledNew.delAngDT >= 2.0f*EKF_TARGET_DT)) { // convert the accumulated quaternion to an equivalent delta angle diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 5969359085..a9af10faa5 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -58,8 +58,8 @@ #define ACCEL_BIAS_LIM_SCALER 0.2f // target update time for the EKF in msec and sec -#define EKF_TARGET_DT_MS 10.0 -#define EKF_TARGET_DT 0.01 +#define EKF_TARGET_DT_MS 12 +#define EKF_TARGET_DT 0.012f class AP_AHRS;