From f1c05ee3200d3654f01578feb1d060d41cfa46a4 Mon Sep 17 00:00:00 2001 From: priseborough Date: Mon, 19 Dec 2016 07:58:27 +1100 Subject: [PATCH] AP_NavEKF3: Allow accel switch on bias uncertainty to be adjusted --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 9 +++++++++ libraries/AP_NavEKF3/AP_NavEKF3.h | 1 + libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 6 +++--- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 6 +++--- 5 files changed, 17 insertions(+), 7 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 8e518c8cd7..bcee1bcc52 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -516,6 +516,15 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = { // @Units: m AP_GROUPINFO("RNG_USE_SPD", 47, NavEKF3, _useRngSwSpd, 2.0f), + // @Param: ACC_BIAS_LIM + // @DisplayName: Accelerometer bias limit + // @Description: The accelerometer bias state will be limited to +- this vlaue + // @Range: 0.5 2.5 + // @Increment: 0.1 + // @User: Advanced + // @Units: m/s/s + AP_GROUPINFO("ACC_BIAS_LIM", 48, NavEKF3, _accBiasLim, 1.0f), + AP_GROUPEND }; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index 6bd5513c90..3938e320b9 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -356,6 +356,7 @@ private: AP_Int16 _rngBcnInnovGate; // Percentage number of standard deviations applied to range beacon innovation consistency check AP_Int8 _rngBcnDelay_ms; // effective average delay of range beacon measurements rel to IMU (msec) AP_Float _useRngSwSpd; // Maximum horizontal ground speed to use range finder as the primary height source (m/s) + AP_Float _accBiasLim; // Accelerometer bias limit (m/s/s) // Tuning parameters const float gpsNEVelVarAccScale; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 2823693366..e873bd1e44 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -142,7 +142,7 @@ void NavEKF3_core::setWindMagStateLearningMode() // activate the states inhibitDelVelBiasStates = false; // set the initial covariance values - P[13][13] = sq(INIT_ACCEL_BIAS_UNCERTAINTY * dtEkfAvg); + P[13][13] = sq(ACCEL_BIAS_LIM_SCALER * frontend->_accBiasLim * dtEkfAvg); P[14][14] = P[13][13]; P[15][15] = P[13][13]; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index daf5c8d941..8471d286d6 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -425,7 +425,7 @@ void NavEKF3_core::CovarianceInit() P[11][11] = P[10][10]; P[12][12] = P[10][10]; // delta velocity biases - P[13][13] = sq(INIT_ACCEL_BIAS_UNCERTAINTY * dtEkfAvg); + P[13][13] = sq(ACCEL_BIAS_LIM_SCALER * frontend->_accBiasLim * dtEkfAvg); P[14][14] = P[13][13]; P[15][15] = P[13][13]; // earth magnetic field @@ -1370,8 +1370,8 @@ void NavEKF3_core::ConstrainStates() stateStruct.position.z = constrain_float(stateStruct.position.z,-4.0e4f,1.0e4f); // gyro bias limit (this needs to be set based on manufacturers specs) for (uint8_t i=10; i<=12; i++) statesArray[i] = constrain_float(statesArray[i],-GYRO_BIAS_LIMIT*dtEkfAvg,GYRO_BIAS_LIMIT*dtEkfAvg); - // accel bias limit 1.0 m/s^2 (this needs to be finalised from test data) - for (uint8_t i=13; i<=15; i++) statesArray[i] = constrain_float(statesArray[i],-1.0f*dtEkfAvg,1.0f*dtEkfAvg); + // the accelerometer bias limit is controlled by a user adjustable parameter + for (uint8_t i=13; i<=15; i++) statesArray[i] = constrain_float(statesArray[i],-frontend->_accBiasLim*dtEkfAvg,frontend->_accBiasLim*dtEkfAvg); // earth magnetic field limit for (uint8_t i=16; i<=18; i++) statesArray[i] = constrain_float(statesArray[i],-1.0f,1.0f); // body magnetic field limit diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index c5ef3aa701..c993b1a98b 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -52,12 +52,12 @@ // assume 3m/s to start #define STARTUP_WIND_SPEED 3.0f -// initial imu bias error (m/s/s) -#define INIT_ACCEL_BIAS_UNCERTAINTY 0.2f - // maximum allowed gyro bias (rad/sec) #define GYRO_BIAS_LIMIT 0.5f +// initial accel bias uncertainty as a fraction of the state limit +#define ACCEL_BIAS_LIM_SCALER 0.2f + class AP_AHRS; class NavEKF3_core