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

AP_NavEKF3: Use large accel process noise when IMU data is bad

This commit is contained in:
Paul Riseborough 2021-07-16 07:32:58 +10:00 committed by Randy Mackay
parent 82ed96a927
commit 36ead940d6
2 changed files with 3 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -1135,7 +1135,7 @@ void NavEKF3_core::CovariancePrediction(Vector3F *rotVarVecPtr)
ftype _gyrNoise = constrain_ftype(frontend->_gyrNoise, 0.0f, 1.0f);
daxVar = dayVar = dazVar = sq(dt*_gyrNoise);
}
ftype _accNoise = constrain_ftype(frontend->_accNoise, 0.0f, 10.0f);
ftype _accNoise = badIMUdata ? BAD_IMU_DATA_ACC_P_NSE : constrain_ftype(frontend->_accNoise, 0.0f, 10.0f);
dvxVar = dvyVar = dvzVar = sq(dt*_accNoise);
if (!inhibitDelVelBiasStates) {

2
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -96,6 +96,8 @@
#define EK3_POSXY_STATE_LIMIT 1.0e6
#endif
#define BAD_IMU_DATA_ACC_P_NSE 5.0f
class NavEKF3_core : public NavEKF_core_common
{
public: