From 6f31961fb52c9f57191932dc103c006572f434e1 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 2 Jan 2014 08:12:06 +1100 Subject: [PATCH] AP_NavEKF: Added height rate adaptive wind speed process noise --- libraries/AP_NavEKF/AP_NavEKF.cpp | 28 +++++++++++++++++++++------- libraries/AP_NavEKF/AP_NavEKF.h | 4 ++++ 2 files changed, 25 insertions(+), 7 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 7e7a6296db..a1374cc282 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -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; diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index e64d5b71fe..4fbe04e11c 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -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