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

AP_NavEKF: Added height rate adaptive wind speed process noise

This commit is contained in:
Paul Riseborough 2014-01-02 08:12:06 +11:00 committed by Andrew Tridgell
parent de884dabab
commit 6f31961fb5
2 changed files with 25 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -21,7 +21,7 @@ NavEKF::NavEKF(const AP_AHRS &ahrs, AP_Baro &baro) :
_baro(baro),
useAirspeed(true),
useCompass(true),
fusionModeGPS(1), // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
fusionModeGPS(0), // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
fuseMeNow(true), // forces fusion to occur on the IMU frame that data arrives
covTimeStepMax(0.07f), // maximum time (sec) between covariance prediction updates
covDelAngMax(0.05f), // maximum delta angle between covariance prediction updates
@ -52,7 +52,11 @@ NavEKF::NavEKF(const AP_AHRS &ahrs, AP_Baro &baro) :
_gpsPosVarAccScale = 0.2f; // scale factor applied to position variance due to Vdot
_magVar = 0.0025f; // magnetometer measurement variance Gauss^2
_magVarRateScale = 0.05f; // scale factor applied to magnetometer variance due to angular rate
_easVar = 2.0f; // equivalent airspeed noise variance (m/s)^2
_windStateNoise = 0.1f; // RMS rate of change of wind (m/s^2)
_wndVarHgtRateScale = 0.5f; // scale factor applied to wind process noise from height rate
// Misc initial conditions
hgtRate = 0.0f;
mag_state.q0 = 1;
mag_state.DCM.identity();
dtIMUAvgInv = 1.0f/dtIMUAvg;
@ -385,7 +389,12 @@ void NavEKF::CovariancePrediction()
float dvz_b;
// calculate covariance prediction process noise
windVelSigma = dt * 0.1f;
// filter height rate using a 10 second time constant filter
float alpha = 0.1f * dt;
hgtRate = hgtRate * (1.0f - alpha) - states[6] * alpha;
// use filtered height rate to increase wind process noise when climbing or descending
// this allows for wind gradient effects.
windVelSigma = dt * _windStateNoise * (1.0f + _wndVarHgtRateScale * fabsf(hgtRate));
dAngBiasSigma = dt * 5.0e-7f;
dVelBiasSigma = dt * 1.0e-4f;
magEarthSigma = dt * 1.5e-4f;
@ -1171,14 +1180,18 @@ void NavEKF::FuseVelPosNED()
for (uint8_t i=3; i<=4; i++) observation[i] = posNE[i-3];
observation[5] = -hgtMea;
// Estimate the GPS Velocity, GPS horiz position and height measurement variances.
// additional error in GPS velocities caused by manoeuvring
// additional error in GPS velocity caused by manoeuvring
velErr = _gpsVelVarAccScale*accNavMag;
// additional error in GPS position caused by manoeuvring
posErr = _gpsPosVarAccScale*accNavMag;
for (uint8_t i=0; i<=2; i++) R_OBS[i] = _gpsHorizVelVar + velErr*velErr;
for (uint8_t i=3; i<=4; i++) R_OBS[i] = _gpsHorizPosVar + posErr*posErr;
// Estimate the GPS Velocity, GPS horiz position and height measurement variances.
R_OBS[0] = _gpsHorizVelVar + sq(velErr);
R_OBS[1] = R_OBS[0];
R_OBS[2] = _gpsVertVelVar + sq(velErr);
R_OBS[3] = _gpsHorizPosVar + sq(posErr);
R_OBS[4] = R_OBS[3];
R_OBS[5] = _gpsVertPosVar;
// Set innovation variances to zero default
@ -1672,7 +1685,8 @@ void NavEKF::FuseAirspeed()
float vd;
float vwn;
float vwe;
const float R_TAS = 2.0f;
float EAS2TAS = _ahrs.get_EAS2TAS();
const float R_TAS = _easVar*sq(constrain_float(EAS2TAS,1.0f,10.0f));
Vector3f SH_TAS;
float SK_TAS;
Vector24 H_TAS;

4
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -191,6 +191,9 @@ private:
float _gpsPosVarAccScale; // scale factor applied to position variance due to Vdot
float _magVar; // magnetometer measurement variance Gauss^2
float _magVarRateScale; // scale factor applied to magnetometer variance due to Vdot
float _easVar; // equivalent airspeed noise variance (m/s)^2
float _windStateNoise; // RMS rate of change of wind (m/s^2)
float _wndVarHgtRateScale; // scale factor applied to wind process noise from height rate
Vector24 states; // state matrix - 4 x quaternions, 3 x Vel, 3 x Pos, 3 x gyro bias, 3 x accel bias, 2 x wind vel, 3 x earth mag field, 3 x body mag field
@ -211,6 +214,7 @@ private:
Vector3f dAngIMU; // delta angle vector in XYZ body axes measured by the IMU (rad)
float dtIMU; // time lapsed since the last IMU measurement (sec)
float dt; // time lapsed since the last covariance prediction (sec)
float hgtRate; // state for rate of change of height filter
bool onGround; // boolean true when the flight vehicle is on the ground (not flying)
const bool useAirspeed; // boolean true if airspeed data is being used
const bool useCompass; // boolean true if magnetometer data is being used