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

AP_NavEKF : add INIT_GYRO_BIAS_UNCERTAINTY definition

Browse Source
This commit is contained in:
priseborough 2014-10-29 19:32:57 +11:00 committed by Randy Mackay
parent ddb8796d2c
commit fecad46336
1 changed files with 5 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -102,6 +102,9 @@ extern const AP_HAL::HAL& hal;
// assume 3m/s to start // assume 3m/s to start
#define STARTUP_WIND_SPEED 3.0f #define STARTUP_WIND_SPEED 3.0f
// initial gyro bias uncertainty (deg/sec)
#define INIT_GYRO_BIAS_UNCERTAINTY 0.1f
// Define tuning parameters // Define tuning parameters
const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = { const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
@ -2816,7 +2819,7 @@ void NavEKF::resetGyroBias(void)
state.gyro_bias.zero(); state.gyro_bias.zero();
zeroRows(P,10,12); zeroRows(P,10,12);
zeroCols(P,10,12); zeroCols(P,10,12);
P[10][10] = sq(radians(0.1f * dtIMU)); P[10][10] = sq(radians(INIT_GYRO_BIAS_UNCERTAINTY * dtIMU));
P[11][11] = P[10][10]; P[11][11] = P[10][10];
P[12][12] = P[10][10]; P[12][12] = P[10][10];
@ -2938,7 +2941,7 @@ void NavEKF::CovarianceInit()
P[8][8] = P[7][7]; P[8][8] = P[7][7];
P[9][9] = sq(5.0f); P[9][9] = sq(5.0f);
// delta angle biases // delta angle biases
P[10][10] = sq(radians(0.1f * dtIMU)); P[10][10] = sq(radians(INIT_GYRO_BIAS_UNCERTAINTY * dtIMU));
P[11][11] = P[10][10]; P[11][11] = P[10][10];
P[12][12] = P[10][10]; P[12][12] = P[10][10];
// Z delta velocity bias // Z delta velocity bias