Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Fix bug in averaged filter delta time 

Average EKF time delta was not being updated.
This commit is contained in:
priseborough 2016-07-04 16:19:13 +10:00 committed by Andrew Tridgell
parent cc7caa27bc
commit 55dee347e0
3 changed files with 8 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -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();

3
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -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;

2
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -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