Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-26 10:38:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Paul Riseborough 2021-07-16 07:32:58 +10:00 committed by Andrew Tridgell
parent eb5a82dd38
commit 8b16d8dc4c
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); ftype _gyrNoise = constrain_ftype(frontend->_gyrNoise, 0.0f, 1.0f);
daxVar = dayVar = dazVar = sq(dt*_gyrNoise); 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); dvxVar = dvyVar = dvzVar = sq(dt*_accNoise);
if (!inhibitDelVelBiasStates) { if (!inhibitDelVelBiasStates) {

2
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

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