Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 15:53:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Allow for larger gyro bias errors

Browse Source 
MPU6000 data sheet indicates that variation on gyro ZRO across temperature range from -40 to +85 is +-20 deg/sec.
The limits on the gyro bias states have been increased to allow for this.
To enable the EKF to accommodate such large gyro bias values in yaw without the yaw error wrapping, leading to continual heading drift, an unwrap function has been applied to the compass heading error.
This commit is contained in:
Paul Riseborough 2015-10-03 14:51:39 -07:00 committed by Randy Mackay
parent 325f4139fe
commit f4db78fc11
2 changed files with 26 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -27,6 +27,9 @@ extern const AP_HAL::HAL& hal;
// initial imu bias uncertainty (deg/sec)
#define INIT_ACCEL_BIAS_UNCERTAINTY 0.3f
// maximum allowed gyro bias (rad/sec)
#define GYRO_BIAS_LIMIT 0.349066f
// constructor
NavEKF2_core::NavEKF2_core(NavEKF2 &_frontend, const AP_AHRS *ahrs, AP_Baro &baro, const RangeFinder &rng) :
frontend(_frontend),
@ -3522,8 +3525,8 @@ void NavEKF2_core::ConstrainStates()
for (uint8_t i=6; i<=7; i++) statesArray[i] = constrain_float(statesArray[i],-1.0e6f,1.0e6f);
// height limit covers home alt on everest through to home alt at SL and ballon drop
stateStruct.position.z = constrain_float(stateStruct.position.z,-4.0e4f,1.0e4f);
// gyro bias limit ~6 deg/sec (this needs to be set based on manufacturers specs)
for (uint8_t i=9; i<=11; i++) statesArray[i] = constrain_float(statesArray[i],-0.1f*dtIMUavg,0.1f*dtIMUavg);
// gyro bias limit (this needs to be set based on manufacturers specs)
for (uint8_t i=9; i<=11; i++) statesArray[i] = constrain_float(statesArray[i],-GYRO_BIAS_LIMIT*dtIMUavg,GYRO_BIAS_LIMIT*dtIMUavg);
// gyro scale factor limit of +-5% (this needs to be set based on manufacturers specs)
for (uint8_t i=12; i<=14; i++) statesArray[i] = constrain_float(statesArray[i],0.95f,1.05f);
// Z accel bias limit 1.0 m/s^2 (this needs to be finalised from test data)
@ -4174,6 +4177,8 @@ void NavEKF2_core::InitialiseVariables()
gpsQualGood = false;
gpsNotAvailable = true;
motorsArmed = false;
innovationIncrement = 0;
lastInnovation = 0;
}
// return true if we should use the airspeed sensor
@ -5014,13 +5019,23 @@ float NavEKF2_core::calcMagHeadingInnov()
float innovation = atan2f(magMeasNED.y,magMeasNED.x) - _ahrs->get_compass()->get_declination();
// wrap the innovation so it sits on the range from +-pi
if (innovation > 3.1415927f) {
innovation = innovation - 6.2831853f;
} else if (innovation < -3.1415927f) {
innovation = innovation + 6.2831853f;
if (innovation > M_PI) {
innovation = innovation - 2*M_PI;
} else if (innovation < -M_PI) {
innovation = innovation + 2*M_PI;
}
return innovation;
// Unwrap so that a large yaw gyro bias offset that causes the heading to wrap does not lead to continual uncontrolled heading drift
if (innovation - lastInnovation > M_PI) {
// Angle has wrapped in the positive direction to subtract an additional 2*Pi
innovationIncrement -= 2*M_PI;
} else if (innovation -innovationIncrement < -M_PI) {
// Angle has wrapped in the negative direction so add an additional 2*Pi
innovationIncrement += 2*M_PI;
}
lastInnovation = innovation;
return innovation + innovationIncrement;
}
#endif // HAL_CPU_CLASS

4
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -702,6 +702,10 @@ private:
bool consistentMagData; // true when the magnetometers are passing consistency checks
bool motorsArmed; // true when the motors have been armed
// States used for unwrapping of compass yaw error
float innovationIncrement;
float lastInnovation;
// variables added for optical flow fusion
of_elements storedOF[OBS_BUFFER_LENGTH]; // OF data buffer
of_elements ofDataNew; // OF data at the current time horizon