aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF : Add time based noise to terrain offset state

Browse Source
This commit is contained in:
priseborough 2014-10-16 16:55:09 +11:00 committed by Andrew Tridgell
parent b1d3d5d9a3
commit 117bd2a998
2 changed files with 5 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -2535,7 +2535,9 @@ void NavEKF::RunAuxiliaryEKF()
return; return;
} }
distanceTravelledSq = min(distanceTravelledSq, 100.0f); distanceTravelledSq = min(distanceTravelledSq, 100.0f);
Popt[1][1] += (distanceTravelledSq * sq(0.01f*float(_gndGradientSigma))); float timeLapsed = min(0.001f * (imuSampleTime_ms - timeAtLastAuxEKF_ms), 1.0f);
Popt[1][1] += (distanceTravelledSq * sq(0.01f*float(_gndGradientSigma))) + sq(1.0f * timeLapsed);
timeAtLastAuxEKF_ms = imuSampleTime_ms;
} }
// fuse range finder data // fuse range finder data
@ -4202,7 +4204,7 @@ void NavEKF::ZeroVariables()
lastFixTime_ms = imuSampleTime_ms; lastFixTime_ms = imuSampleTime_ms;
secondLastFixTime_ms = imuSampleTime_ms; secondLastFixTime_ms = imuSampleTime_ms;
lastDecayTime_ms = imuSampleTime_ms; lastDecayTime_ms = imuSampleTime_ms;
airborneDetectTime_ms = imuSampleTime_ms; timeAtLastAuxEKF_ms = imuSampleTime_ms;
gpsNoiseScaler = 1.0f; gpsNoiseScaler = 1.0f;
velTimeout = false; velTimeout = false;

1
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -503,6 +503,7 @@ private:
uint8_t storeIndex; // State vector storage index uint8_t storeIndex; // State vector storage index
uint32_t lastStateStoreTime_ms; // time of last state vector storage uint32_t lastStateStoreTime_ms; // time of last state vector storage
uint32_t lastFixTime_ms; // time of last GPS fix used to determine if new data has arrived uint32_t lastFixTime_ms; // time of last GPS fix used to determine if new data has arrived
uint32_t timeAtLastAuxEKF_ms; // last time the auxilliary filter was run to fuse range or optical flow measurements
uint32_t secondLastFixTime_ms; // time of second last GPS fix used to determine how long since last update uint32_t secondLastFixTime_ms; // time of second last GPS fix used to determine how long since last update
uint32_t lastHealthyMagTime_ms; // time the magnetometer was last declared healthy uint32_t lastHealthyMagTime_ms; // time the magnetometer was last declared healthy
Vector3f lastAngRate; // angular rate from previous IMU sample used for trapezoidal integrator Vector3f lastAngRate; // angular rate from previous IMU sample used for trapezoidal integrator